merge: resolve xtkuang_dev into dev
This commit is contained in:
commit
f443a7ce53
3
.gitignore
vendored
3
.gitignore
vendored
@ -2,7 +2,8 @@
|
|||||||
/cmake-build-debug
|
/cmake-build-debug
|
||||||
/cmake-build-sigma-debuggit
|
/cmake-build-sigma-debuggit
|
||||||
/build
|
/build
|
||||||
|
/build-*
|
||||||
/log
|
/log
|
||||||
/third_party/osqp/
|
/third_party/osqp/
|
||||||
/third_party/OsqpEigen/
|
/third_party/OsqpEigen/
|
||||||
/output
|
/output
|
||||||
|
|||||||
6
.vscode/extensions.json
vendored
6
.vscode/extensions.json
vendored
@ -1,6 +0,0 @@
|
|||||||
{
|
|
||||||
"recommendations": [
|
|
||||||
"ms-vscode.cpptools",
|
|
||||||
"ms-vscode.cmake-tools"
|
|
||||||
]
|
|
||||||
}
|
|
||||||
40
.vscode/launch.json
vendored
40
.vscode/launch.json
vendored
@ -1,32 +1,30 @@
|
|||||||
{
|
{
|
||||||
|
"version": "0.2.0",
|
||||||
"configurations": [
|
"configurations": [
|
||||||
|
|
||||||
{
|
{
|
||||||
"name": "C/C++: g++ 构建和调试活动文件",
|
"name": "Debug cmvr_es (installed output)",
|
||||||
"type": "cppdbg",
|
"type": "cppdbg",
|
||||||
"request": "launch",
|
"request": "launch",
|
||||||
"program": "${fileDirname}/${fileBasenameNoExtension}",
|
"program": "${workspaceFolder}/output/bin/cmvr_es",
|
||||||
"args": [],
|
"args": [],
|
||||||
"stopAtEntry": false,
|
"stopAtEntry": false,
|
||||||
"cwd": "${fileDirname}",
|
"cwd": "${workspaceFolder}/output/bin",
|
||||||
"environment": [],
|
"environment": [
|
||||||
"externalConsole": false,
|
|
||||||
"MIMode": "gdb",
|
|
||||||
"setupCommands": [
|
|
||||||
{
|
{
|
||||||
"description": "为 gdb 启用整齐打印",
|
"name": "LD_LIBRARY_PATH",
|
||||||
"text": "-enable-pretty-printing",
|
"value": "${workspaceFolder}/output/lib:${env:LD_LIBRARY_PATH}"
|
||||||
"ignoreFailures": true
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"description": "将反汇编风格设置为 Intel",
|
|
||||||
"text": "-gdb-set disassembly-flavor intel",
|
|
||||||
"ignoreFailures": true
|
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"preLaunchTask": "C/C++: g++ 生成活动文件",
|
"externalConsole": false,
|
||||||
"miDebuggerPath": "/usr/bin/gdb"
|
"MIMode": "gdb",
|
||||||
|
"preLaunchTask": "cmake: install",
|
||||||
|
"setupCommands": [
|
||||||
|
{
|
||||||
|
"description": "Enable pretty-printing for gdb",
|
||||||
|
"text": "-enable-pretty-printing",
|
||||||
|
"ignoreFailures": true
|
||||||
|
}
|
||||||
|
]
|
||||||
}
|
}
|
||||||
],
|
]
|
||||||
"version": "2.0.0"
|
}
|
||||||
}
|
|
||||||
|
|||||||
19
.vscode/settings.json
vendored
19
.vscode/settings.json
vendored
@ -1,12 +1,11 @@
|
|||||||
{
|
{
|
||||||
"cmake.configureOnOpen": true,
|
"cmake.sourceDirectory": "${workspaceFolder}",
|
||||||
"cmake.buildDirectory": "${workspaceFolder}/cmake-build-debug",
|
"cmake.buildDirectory": "${workspaceFolder}/build/vscode-debug",
|
||||||
"cmake.copyCompileCommands": "${workspaceFolder}/compile_commands.json",
|
"cmake.configureSettings": {
|
||||||
"cmake.configureSettings": {
|
"CMAKE_BUILD_TYPE": "Debug",
|
||||||
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
|
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
|
||||||
},
|
},
|
||||||
"C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
|
"C_Cpp.default.compileCommands": "${workspaceFolder}/build/vscode-debug/compile_commands.json",
|
||||||
"C_Cpp.default.compileCommands": "${workspaceFolder}/compile_commands.json",
|
"C_Cpp.default.cppStandard": "c++17",
|
||||||
"C_Cpp.default.cppStandard": "c++17",
|
"C_Cpp.default.intelliSenseMode": "linux-gcc-x64"
|
||||||
"C_Cpp.default.intelliSenseMode": "linux-gcc-x64"
|
|
||||||
}
|
}
|
||||||
|
|||||||
74
.vscode/tasks.json
vendored
74
.vscode/tasks.json
vendored
@ -1,25 +1,71 @@
|
|||||||
{
|
{
|
||||||
|
"version": "2.0.0",
|
||||||
"tasks": [
|
"tasks": [
|
||||||
{
|
{
|
||||||
"type": "cppbuild",
|
"label": "cmake: configure",
|
||||||
"label": "C/C++: g++ 生成活动文件",
|
"type": "shell",
|
||||||
"command": "/usr/bin/g++",
|
"command": "cmake",
|
||||||
"args": [
|
"args": [
|
||||||
"-fdiagnostics-color=always",
|
"-S",
|
||||||
"-g",
|
"${workspaceFolder}",
|
||||||
"${file}",
|
"-B",
|
||||||
"-o",
|
"${workspaceFolder}/build/vscode-debug",
|
||||||
"${fileDirname}/${fileBasenameNoExtension}"
|
"-DCMAKE_BUILD_TYPE=Debug",
|
||||||
|
"-DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
|
||||||
],
|
],
|
||||||
"options": {
|
"group": "build",
|
||||||
"cwd": "${fileDirname}"
|
"problemMatcher": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"label": "cmake: build",
|
||||||
|
"type": "shell",
|
||||||
|
"command": "cmake",
|
||||||
|
"args": [
|
||||||
|
"--build",
|
||||||
|
"${workspaceFolder}/build/vscode-debug",
|
||||||
|
"-j",
|
||||||
|
"8"
|
||||||
|
],
|
||||||
|
"dependsOn": [
|
||||||
|
"cmake: configure"
|
||||||
|
],
|
||||||
|
"group": {
|
||||||
|
"kind": "build",
|
||||||
|
"isDefault": true
|
||||||
},
|
},
|
||||||
"problemMatcher": [
|
"problemMatcher": [
|
||||||
"$gcc"
|
"$gcc"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"label": "cmake: install",
|
||||||
|
"type": "shell",
|
||||||
|
"command": "cmake",
|
||||||
|
"args": [
|
||||||
|
"--install",
|
||||||
|
"${workspaceFolder}/build/vscode-debug",
|
||||||
|
"--verbose"
|
||||||
|
],
|
||||||
|
"dependsOn": [
|
||||||
|
"cmake: build"
|
||||||
],
|
],
|
||||||
"group": "build",
|
"group": "build",
|
||||||
"detail": "调试器生成的任务。"
|
"problemMatcher": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"label": "cmvr_es: run installed",
|
||||||
|
"type": "shell",
|
||||||
|
"command": "${workspaceFolder}/output/bin/cmvr_es",
|
||||||
|
"options": {
|
||||||
|
"cwd": "${workspaceFolder}/output/bin",
|
||||||
|
"env": {
|
||||||
|
"LD_LIBRARY_PATH": "${workspaceFolder}/output/lib:${env:LD_LIBRARY_PATH}"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"dependsOn": [
|
||||||
|
"cmake: install"
|
||||||
|
],
|
||||||
|
"problemMatcher": []
|
||||||
}
|
}
|
||||||
],
|
]
|
||||||
"version": "2.0.0"
|
}
|
||||||
}
|
|
||||||
|
|||||||
@ -110,7 +110,7 @@ target_link_libraries(cmvr_es PRIVATE
|
|||||||
cmvr_es::runtime
|
cmvr_es::runtime
|
||||||
cmvr_es::proto
|
cmvr_es::proto
|
||||||
cmvr_es::logging
|
cmvr_es::logging
|
||||||
service
|
cmvr_es::quic_edge_task
|
||||||
${GLOG_LIBRARIES}
|
${GLOG_LIBRARIES}
|
||||||
jsoncpp
|
jsoncpp
|
||||||
cmvr_es::service
|
cmvr_es::service
|
||||||
|
|||||||
@ -63,6 +63,7 @@ function(setup_external_libs ARCH)
|
|||||||
"${FULL_PATH}/lib/*.so"
|
"${FULL_PATH}/lib/*.so"
|
||||||
"${FULL_PATH}/lib/*.so.*"
|
"${FULL_PATH}/lib/*.so.*"
|
||||||
)
|
)
|
||||||
|
list(FILTER _SO_FILES EXCLUDE REGEX "/libstdc\\+\\+\\.so(\\..*)?$")
|
||||||
if(_SO_FILES)
|
if(_SO_FILES)
|
||||||
list(APPEND INSTALL_SO_FILES ${_SO_FILES})
|
list(APPEND INSTALL_SO_FILES ${_SO_FILES})
|
||||||
endif()
|
endif()
|
||||||
@ -85,6 +86,7 @@ function(setup_external_libs ARCH)
|
|||||||
"${FULL_PATH}/*.so"
|
"${FULL_PATH}/*.so"
|
||||||
"${FULL_PATH}/*.so.*"
|
"${FULL_PATH}/*.so.*"
|
||||||
)
|
)
|
||||||
|
list(FILTER _SO_FILES2 EXCLUDE REGEX "/libstdc\\+\\+\\.so(\\..*)?$")
|
||||||
if(_SO_FILES2)
|
if(_SO_FILES2)
|
||||||
list(APPEND INSTALL_SO_FILES ${_SO_FILES2})
|
list(APPEND INSTALL_SO_FILES ${_SO_FILES2})
|
||||||
endif()
|
endif()
|
||||||
@ -138,6 +140,14 @@ function(setup_external_libs ARCH)
|
|||||||
)
|
)
|
||||||
|
|
||||||
install(FILES ${INSTALL_SO_FILES} DESTINATION lib)
|
install(FILES ${INSTALL_SO_FILES} DESTINATION lib)
|
||||||
|
install(CODE [[
|
||||||
|
file(GLOB _bundled_stdlib_files
|
||||||
|
"${CMAKE_INSTALL_PREFIX}/lib/libstdc++.so"
|
||||||
|
"${CMAKE_INSTALL_PREFIX}/lib/libstdc++.so.*")
|
||||||
|
if(_bundled_stdlib_files)
|
||||||
|
file(REMOVE ${_bundled_stdlib_files})
|
||||||
|
endif()
|
||||||
|
]])
|
||||||
|
|
||||||
# After installing, patch RPATH of installed shared libs to $ORIGIN
|
# After installing, patch RPATH of installed shared libs to $ORIGIN
|
||||||
install(CODE [[
|
install(CODE [[
|
||||||
|
|||||||
16
cmake/msquic/ProcessorCount.cmake
Normal file
16
cmake/msquic/ProcessorCount.cmake
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
# MsQuic's QuicTLS custom build asks CMake's ProcessorCount module for a
|
||||||
|
# nested `make -jN` value. Route that query through the build script's
|
||||||
|
# --jobs setting so the dependency build cannot silently oversubscribe the
|
||||||
|
# host.
|
||||||
|
|
||||||
|
if(NOT DEFINED CMVR_MSQUIC_PROCESSOR_COUNT OR
|
||||||
|
NOT CMVR_MSQUIC_PROCESSOR_COUNT MATCHES "^[1-9][0-9]*$")
|
||||||
|
message(FATAL_ERROR
|
||||||
|
"CMVR_MSQUIC_PROCESSOR_COUNT must be a positive integer")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
function(ProcessorCount result_variable)
|
||||||
|
set("${result_variable}"
|
||||||
|
"${CMVR_MSQUIC_PROCESSOR_COUNT}"
|
||||||
|
PARENT_SCOPE)
|
||||||
|
endfunction()
|
||||||
@ -7,7 +7,10 @@ add_subdirectory(algorithms)
|
|||||||
add_subdirectory(simulate)
|
add_subdirectory(simulate)
|
||||||
add_subdirectory(devices)
|
add_subdirectory(devices)
|
||||||
add_subdirectory(manager/device_manager)
|
add_subdirectory(manager/device_manager)
|
||||||
|
add_subdirectory(manager/media_source_hub)
|
||||||
|
add_subdirectory(service/quic_edge)
|
||||||
add_subdirectory(task)
|
add_subdirectory(task)
|
||||||
|
add_subdirectory(task/quic_edge_task)
|
||||||
add_subdirectory(manager/task_manager)
|
add_subdirectory(manager/task_manager)
|
||||||
add_subdirectory(service)
|
add_subdirectory(service)
|
||||||
add_subdirectory(runtime)
|
add_subdirectory(runtime)
|
||||||
|
|||||||
125
cmvr-es/algorithms/README.md
Normal file
125
cmvr-es/algorithms/README.md
Normal file
@ -0,0 +1,125 @@
|
|||||||
|
# Algorithms 模块开发指南
|
||||||
|
|
||||||
|
`algorithms/` 保存与具体厂商协议无关的运动学、规划、控制和感知算法。算法接受通用类型或显式接口输入，不应直接解析设备报文，也不应承担 gRPC/QUIC 传输职责。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 当前结构
|
||||||
|
|
||||||
|
| 目录 | 主要能力 | 主要 CMake target |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `kinematics/ik_solver/` | Pinocchio DLS/QP、SRS、LAWBA 逆运动学 | `cmvr_es::ik_solver` |
|
||||||
|
| `motion_planner/base_motion/` | TOPPRA、S 曲线、笛卡尔速度限制 | `cmvr_es::base_motion` |
|
||||||
|
| `motion_planner/arm_motion/` | MoveJ、MoveL、SpeedL 机械臂规划 | `cmvr_es::algorithms::arm_motion` |
|
||||||
|
| `controllers/` | PID、IBVS、笛卡尔速度控制 | `cmvr_es::algorithms::controller`、`cmvr_es::algorithms::arm_control` |
|
||||||
|
| `perception/` | AprilTag 和视觉定位 | `cmvr_es::perception` |
|
||||||
|
|
||||||
|
顶层入口是 [`CMakeLists.txt`](CMakeLists.txt)。
|
||||||
|
|
||||||
|
## 依赖边界
|
||||||
|
|
||||||
|
- 算法层可以依赖 `common/`、Eigen、Pinocchio、OSQP、TOPPRA、OpenCV、ViSP 等；
|
||||||
|
- 不包含串口、CAN、HTTP 或厂商 SDK 协议处理；
|
||||||
|
- 不启动 gRPC/QUIC 服务或管理设备生命周期；
|
||||||
|
- 不从算法内部读取全局配置文件，构造或 `configure` 时显式传入配置；
|
||||||
|
- 可复用算法不应主动取得 `DeviceManager` 单例。
|
||||||
|
|
||||||
|
当前部分 controller target 仍链接 `device_manager`，这是现有耦合。新增算法应优先通过参数、回调或窄接口注入设备状态，避免继续扩大该依赖。
|
||||||
|
|
||||||
|
## 扩展已有算法类别
|
||||||
|
|
||||||
|
### 1. 定义或复用抽象接口
|
||||||
|
|
||||||
|
常用接口：
|
||||||
|
|
||||||
|
- [`IKSolver`](kinematics/ik_solver/common/include/ik_solver.h)
|
||||||
|
- [`JointMotionPlanner`](motion_planner/arm_motion/joint_motion/joint_motion_planner.h)
|
||||||
|
- [`CartesianMotionPlanner`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h)
|
||||||
|
|
||||||
|
接口应明确：
|
||||||
|
|
||||||
|
- 输入输出单位和坐标系；
|
||||||
|
- 是否修改内部状态；
|
||||||
|
- 是否线程安全；
|
||||||
|
- 失败时输出是否保持不变；
|
||||||
|
- 是否支持实时循环，以及最大允许耗时。
|
||||||
|
|
||||||
|
### 2. 增加配置
|
||||||
|
|
||||||
|
在 [`../../protos/README.md`](../../protos/README.md) 指导下：
|
||||||
|
|
||||||
|
1. 为算法增加独立配置 message；
|
||||||
|
2. 在所属 `oneof algorithm` 中增加新字段和新 tag；
|
||||||
|
3. 不复用已发布 tag；
|
||||||
|
4. 为迭代次数、容差、速度和加速度设置有效范围；
|
||||||
|
5. 在默认设备配置中给出显式参数。
|
||||||
|
|
||||||
|
### 3. 实现与工厂注册
|
||||||
|
|
||||||
|
将实现放在对应类别子目录，并修改实际工厂：
|
||||||
|
|
||||||
|
- IK：[`ik_solver_factory.h`](kinematics/ik_solver/ik_solver_factory.h)
|
||||||
|
- MoveJ：[`joint_motion_planner_factory.h`](motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h)
|
||||||
|
- MoveL / SpeedL：[`cartesian_motion_planner_factory.h`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h)
|
||||||
|
|
||||||
|
工厂失败应返回 `nullptr` 并记录清晰原因，不能静默回退到另一个算法。MoveL 和 SpeedL 的实现必须保持配置组合一致。
|
||||||
|
|
||||||
|
### 4. 更新 CMake
|
||||||
|
|
||||||
|
- 将实现 `.cpp` 加入对应 library；
|
||||||
|
- 使用项目已有 alias target；
|
||||||
|
- 通过 `target_include_directories` 暴露公共头；
|
||||||
|
- 将依赖放入使用它的最小 target；
|
||||||
|
- 测试源文件不能加入生产共享库；
|
||||||
|
- 新增三方依赖时同步根依赖发现逻辑和 `request.txt`。
|
||||||
|
|
||||||
|
## 数值与机器人语义
|
||||||
|
|
||||||
|
算法扩展至少需要明确：
|
||||||
|
|
||||||
|
- 关节位置单位为 rad，速度为 rad/s；
|
||||||
|
- 笛卡尔平移为 m，旋转和角速度为 rad；
|
||||||
|
- base、tool、world、user frame 的转换方向；
|
||||||
|
- URDF base frame、tip frame 和关节顺序；
|
||||||
|
- 位置、速度、加速度和 jerk 限制；
|
||||||
|
- 奇异点、不可达目标和求解超时行为；
|
||||||
|
- measured state 与算法内部 seed 的更新时机。
|
||||||
|
|
||||||
|
IK 在求解前应使用真实关节角更新 seed。MoveL 连续求解时，应使用上一步解更新下一步状态，不能一直使用初始状态。
|
||||||
|
|
||||||
|
## 测试要求
|
||||||
|
|
||||||
|
每个新算法至少覆盖：
|
||||||
|
|
||||||
|
1. 正常输入；
|
||||||
|
2. 空输入、自由度不匹配和 NaN/Inf；
|
||||||
|
3. 关节限位与速度限制；
|
||||||
|
4. 不可达目标和不收敛；
|
||||||
|
5. 坐标系转换；
|
||||||
|
6. 确定性和重复调用；
|
||||||
|
7. 若用于实时控制，统计最坏执行时间；
|
||||||
|
8. 与一个已知模型或离线参考结果对比。
|
||||||
|
|
||||||
|
当前不少算法测试只通过 `add_executable()` 构建，没有登记到 CTest。新增无设备测试应放在 `BUILD_TESTING` 条件内，并使用 `add_test()`；需要图形界面、RealSense 或 MuJoCo 的测试应明确标为集成测试，不得阻塞默认无设备测试。
|
||||||
|
|
||||||
|
## 新增算法类别
|
||||||
|
|
||||||
|
如果现有类别无法承载：
|
||||||
|
|
||||||
|
1. 在 `algorithms/<category>/` 新建目录；
|
||||||
|
2. 定义协议无关抽象接口；
|
||||||
|
3. 定义配置 Proto 和工厂；
|
||||||
|
4. 提供单独 CMake library 与 `cmvr_es::...` alias；
|
||||||
|
5. 在 [`algorithms/CMakeLists.txt`](CMakeLists.txt) 添加子目录；
|
||||||
|
6. 由设备或任务层注入使用，不让算法反向控制服务层；
|
||||||
|
7. 添加无设备单元测试和真实设备/仿真集成测试。
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] 厂商协议没有进入算法接口
|
||||||
|
- [ ] 单位、坐标系和关节顺序明确
|
||||||
|
- [ ] 工厂已注册且配置组合经过校验
|
||||||
|
- [ ] 不可达、超时和数值异常可观测
|
||||||
|
- [ ] 测试没有被编入生产共享库
|
||||||
|
- [ ] 无设备测试已登记到 CTest
|
||||||
|
- [ ] 实时路径没有日志洪泛和无界内存分配
|
||||||
111
cmvr-es/common/README.md
Normal file
111
cmvr-es/common/README.md
Normal file
@ -0,0 +1,111 @@
|
|||||||
|
# Common 模块开发指南
|
||||||
|
|
||||||
|
`common/` 保存可被设备、算法、管理器和协议层复用的基础能力。这里适合放稳定、协议无关、厂商无关的类型与工具，不适合放设备连接、业务服务或任务调度逻辑。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 目录职责
|
||||||
|
|
||||||
|
| 目录 | 职责 |
|
||||||
|
| --- | --- |
|
||||||
|
| `base/` | 日志、基础常量、gRPC 辅助函数和线程安全缓冲区 |
|
||||||
|
| `config/` | 配置根目录解析和 Proto Text 配置加载 |
|
||||||
|
| `io/` | Protobuf 二进制与 TextFormat 文件读写 |
|
||||||
|
| `math/` | 坐标变换、关节限制、QP 和运动数学 |
|
||||||
|
| `media/` | 协议无关媒体模型以及 FFmpeg 采集、编码、写文件能力 |
|
||||||
|
| `types/` | 跨后端共享的领域类型，例如 AGV、机械臂和几何类型 |
|
||||||
|
| `vision/` | 图像显示、投影等视觉辅助代码 |
|
||||||
|
|
||||||
|
## 依赖边界
|
||||||
|
|
||||||
|
新增公共组件时应遵守：
|
||||||
|
|
||||||
|
- 不依赖 `service/`、`task/` 或具体厂商设备实现；
|
||||||
|
- 不保存 gRPC/QUIC 连接、session 或客户端状态；
|
||||||
|
- 通用类型不包含厂商报文字段、端口号和私有错误码；
|
||||||
|
- 需要调用设备的逻辑应放在 manager adapter、service 或 task；
|
||||||
|
- 需要第三方库的 `.cpp` 组件应通过明确的 CMake target 暴露依赖；
|
||||||
|
- 避免在公共头文件中使用全局 `using namespace` 或引入大体量实现头。
|
||||||
|
|
||||||
|
当前 `common` 共享库目标是 `cmvr_es::common`，日志是独立目标 `cmvr_es::logging`。新增 `.cpp` 文件时，需要更新 [`CMakeLists.txt`](CMakeLists.txt) 或对应子目录 CMake；纯头文件不需要加入 `add_library` 源文件列表。
|
||||||
|
|
||||||
|
## 新增共享类型
|
||||||
|
|
||||||
|
1. 选择 `types/<domain>/` 或已有领域文件；
|
||||||
|
2. 类型使用明确单位，例如米、弧度、秒、纳秒；
|
||||||
|
3. 为容器长度、自由度和数值范围提供校验函数；
|
||||||
|
4. 保持控制器无关，将厂商字段转换为通用枚举或结果；
|
||||||
|
5. 确认不会迫使所有调用方引入设备 SDK；
|
||||||
|
6. 增加边界值和错误输入测试。
|
||||||
|
|
||||||
|
AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机械臂通用类型应参考 [`types/arm/arm_types.h`](types/arm/arm_types.h)。不要为了一个具体控制器把协议结构塞回 `abstract_*.h`。
|
||||||
|
|
||||||
|
## 媒体模型
|
||||||
|
|
||||||
|
[`media/media_frame.h`](media/media_frame.h) 中的 `TrackDescriptor`、`MediaFrame` 及 payload 在构造后不可变，可被多个协议消费者共享。
|
||||||
|
|
||||||
|
扩展媒体字段时需要保持：
|
||||||
|
|
||||||
|
- `TrackDescriptor::generation` 非零，编码参数变化时创建新 descriptor；
|
||||||
|
- PTS、DTS 和 duration 使用 descriptor 的 `time_base`；
|
||||||
|
- `capture_time_ns` 使用单调时钟，供节奏控制和延迟统计；
|
||||||
|
- `capture_utc_ns` 只作为可选墙上时间，不能用于计算持续时间；
|
||||||
|
- H.264/H.265 明确 `ANNEX_B` 或 `AVCC`；
|
||||||
|
- AAC、Opus、PCM 明确 payload format、采样率和声道数；
|
||||||
|
- 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。
|
||||||
|
|
||||||
|
设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。
|
||||||
|
|
||||||
|
## 环形队列选择
|
||||||
|
|
||||||
|
[`base/ring_buffer.h`](base/ring_buffer.h) 当前包含三类缓冲区：
|
||||||
|
|
||||||
|
| 类型 | 使用场景 | 重要约束 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| `RingBuffer<T>` | 只需要保存最近 N 项并批量读取 | 覆盖最旧项，没有阻塞读取 |
|
||||||
|
| `SPMCRingBuffer<T>` | 历史单生产者场景 | 独立 `reader_tail` 只能由一个线程拥有 |
|
||||||
|
| `BroadcastFrameRing<T>` | 新的媒体或广播式多消费者场景 | 每个消费者使用独立 Cursor，保存不可变共享对象 |
|
||||||
|
|
||||||
|
新的实时多消费者模块优先使用 `BroadcastFrameRing<T>`：
|
||||||
|
|
||||||
|
- capacity 必须大于零；
|
||||||
|
- 同一 Cursor 不得被多个线程同时读取或移动；
|
||||||
|
- 慢消费者落后时会跳到最旧保留项，并得到精确 dropped count；
|
||||||
|
- `reset()` 开启新 generation，旧 Cursor 在下一次成功读取时看到变化；
|
||||||
|
- `close()` 唤醒等待者，关闭后不能继续发布；
|
||||||
|
- 不要先读取 head 再无锁读取槽位，应使用队列提供的原子读取接口。
|
||||||
|
|
||||||
|
## 配置和文件路径
|
||||||
|
|
||||||
|
[`config/config_files.h`](config/config_files.h) 提供：
|
||||||
|
|
||||||
|
- `resolveConfigFile()`：相对根配置目录解析业务配置；
|
||||||
|
- `resolveResourceFile()`：在配置根及其父目录中查找模型等资源；
|
||||||
|
- `loadConfigFile()` / `saveConfigFile()`：读写 Proto Text 配置。
|
||||||
|
|
||||||
|
进程启动后配置根由 `main.cpp` 设置。公共组件不应自行使用当前工作目录拼接配置路径。
|
||||||
|
|
||||||
|
[`io/proto_file_io.h`](io/proto_file_io.h) 写出的 TextFormat 文件权限为 `0600`。保存运行时配置前，应确认目标目录存在，并避免把生产密钥写入仓库。
|
||||||
|
|
||||||
|
## 新增公共组件
|
||||||
|
|
||||||
|
1. 确认能力确实会被两个及以上模块复用；
|
||||||
|
2. 定义最小 API 和所有权、线程安全、错误语义；
|
||||||
|
3. 将头文件放入合适子目录，将实现放入相邻 `.cpp`；
|
||||||
|
4. 更新 CMake target 和 `target_link_libraries`；
|
||||||
|
5. 不使用未声明的传递依赖；
|
||||||
|
6. 增加无设备单元测试；
|
||||||
|
7. 对并发组件增加关闭、超时、覆盖、取消和析构测试；
|
||||||
|
8. 使用 ASan/TSan 时检查生命周期和数据竞争。
|
||||||
|
|
||||||
|
推荐测试目标放在组件相邻的 `tests/`，并在 `BUILD_TESTING` 下通过 `add_test()` 登记。仅创建 `_test` 可执行文件不会自动进入 CTest。
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] API 不依赖具体设备或传输协议
|
||||||
|
- [ ] 公共类型有明确单位和有效性规则
|
||||||
|
- [ ] 所有权及线程安全写入注释
|
||||||
|
- [ ] 新增 `.cpp` 和依赖已经加入 CMake
|
||||||
|
- [ ] 缓冲区关闭能够唤醒等待线程
|
||||||
|
- [ ] 不记录密码、私钥或大块媒体 payload
|
||||||
|
- [ ] 无设备测试可以在开发主机运行
|
||||||
@ -6,16 +6,25 @@
|
|||||||
#define CMVR_ES_RING_BUFFER_H
|
#define CMVR_ES_RING_BUFFER_H
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
#include <chrono>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <cstdint>
|
||||||
#include <deque>
|
#include <deque>
|
||||||
|
#include <memory>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <vector>
|
|
||||||
#include <atomic>
|
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
template<typename T>
|
template<typename T>
|
||||||
class RingBuffer {
|
class RingBuffer {
|
||||||
public:
|
public:
|
||||||
explicit RingBuffer(size_t capacity) : capacity_(capacity) {}
|
explicit RingBuffer(size_t capacity) : capacity_(capacity) {
|
||||||
|
if (capacity_ == 0) {
|
||||||
|
throw std::invalid_argument("RingBuffer capacity must be greater than zero");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void push(const T& item) {
|
void push(const T& item) {
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
@ -50,47 +59,49 @@ template<typename T>
|
|||||||
class SPMCRingBuffer {
|
class SPMCRingBuffer {
|
||||||
public:
|
public:
|
||||||
explicit SPMCRingBuffer(size_t capacity)
|
explicit SPMCRingBuffer(size_t capacity)
|
||||||
: buffer_(capacity), capacity_(capacity),
|
: buffer_(capacity), capacity_(capacity) {
|
||||||
head_(0), tail_(0) {}
|
if (capacity_ == 0) {
|
||||||
|
throw std::invalid_argument("SPMCRingBuffer capacity must be greater than zero");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// 写入操作（仅支持单个生产者）
|
// 写入操作（仅支持单个生产者）
|
||||||
void push(const T& item) {
|
void push(const T& item) {
|
||||||
size_t head = head_.load(std::memory_order_relaxed);
|
{
|
||||||
size_t tail = tail_.load(std::memory_order_acquire);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
buffer_[head % capacity_] = item;
|
buffer_[head_ % capacity_] = item;
|
||||||
head = head + 1;
|
++head_;
|
||||||
head_.store(head, std::memory_order_release);
|
if (head_ - tail_ > capacity_) {
|
||||||
if (head - tail >= capacity_) {
|
// 队列满，覆盖最旧的数据
|
||||||
// 队列满，覆盖最旧的数据
|
tail_ = head_ - capacity_;
|
||||||
tail_.store(tail + 1, std::memory_order_release);
|
}
|
||||||
}
|
}
|
||||||
|
condition_.notify_all();
|
||||||
}
|
}
|
||||||
|
|
||||||
// 单消费者使用（内部 tail_）
|
// 单消费者使用（内部 tail_）
|
||||||
std::optional<T> pop() {
|
std::optional<T> pop() {
|
||||||
size_t tail = tail_.load(std::memory_order_relaxed);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
size_t head = head_.load(std::memory_order_acquire);
|
if (tail_ >= head_) return std::nullopt;
|
||||||
if (tail >= head) return std::nullopt;
|
T value = buffer_[tail_ % capacity_];
|
||||||
T value = buffer_[tail % capacity_];
|
++tail_;
|
||||||
tail_.store(tail + 1, std::memory_order_release);
|
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::optional<T> getLast() {
|
std::optional<T> getLast() const {
|
||||||
size_t tail = tail_.load(std::memory_order_relaxed);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
size_t head = head_.load(std::memory_order_acquire);
|
if (tail_ >= head_) return std::nullopt;
|
||||||
if (tail >= head) return std::nullopt;
|
return buffer_[(head_ - 1) % capacity_];
|
||||||
T value = buffer_[head_ % capacity_];
|
|
||||||
return value;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 多消费者使用（每个读者独立维护 reader_tail）
|
// 多消费者使用（每个读者独立维护 reader_tail）。同一个 reader_tail 只能由
|
||||||
|
// 一个消费线程拥有，且不要把该游标与无参 pop() 的共享 tail_ 混合作为同一路读取。
|
||||||
std::optional<T> pop(size_t& reader_tail) const {
|
std::optional<T> pop(size_t& reader_tail) const {
|
||||||
size_t head = head_.load(std::memory_order_acquire);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
if (reader_tail >= head) return std::nullopt;
|
if (reader_tail >= head_) return std::nullopt;
|
||||||
if (head > reader_tail + capacity_) {
|
if (reader_tail < tail_) {
|
||||||
// 数据已被覆盖，跳过无效读取区间
|
// 数据已被覆盖，跳过无效读取区间
|
||||||
reader_tail = head - capacity_;
|
reader_tail = tail_;
|
||||||
return std::nullopt;
|
return std::nullopt;
|
||||||
}
|
}
|
||||||
T value = buffer_[reader_tail % capacity_];
|
T value = buffer_[reader_tail % capacity_];
|
||||||
@ -98,16 +109,52 @@ public:
|
|||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 在同一次加锁中把独立读游标跳到当前最新元素并读取，避免先 getHead()
|
||||||
|
// 再 pop() 时被高速覆盖造成的检查/读取竞态。
|
||||||
|
std::optional<T> getLatest(size_t& reader_tail) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (tail_ >= head_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
reader_tail = head_ - 1;
|
||||||
|
T value = buffer_[reader_tail % capacity_];
|
||||||
|
++reader_tail;
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<class Rep, class Period>
|
||||||
|
std::optional<T> waitPop(
|
||||||
|
size_t& reader_tail,
|
||||||
|
const std::chrono::duration<Rep, Period>& timeout) const {
|
||||||
|
std::unique_lock<std::mutex> lock(mutex_);
|
||||||
|
condition_.wait_for(lock, timeout, [&] { return reader_tail < head_; });
|
||||||
|
if (reader_tail >= head_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
if (reader_tail < tail_) {
|
||||||
|
reader_tail = tail_;
|
||||||
|
}
|
||||||
|
if (reader_tail >= head_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
T value = buffer_[reader_tail % capacity_];
|
||||||
|
++reader_tail;
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
size_t size() const {
|
size_t size() const {
|
||||||
return head_.load(std::memory_order_acquire) - tail_.load(std::memory_order_acquire);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return head_ - tail_;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t getHead() const {
|
size_t getHead() const {
|
||||||
return head_.load(std::memory_order_acquire);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return head_;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t getTail() const {
|
size_t getTail() const {
|
||||||
return tail_.load(std::memory_order_acquire);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return tail_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool empty() const {
|
bool empty() const {
|
||||||
@ -119,16 +166,278 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
void clear() {
|
void clear() {
|
||||||
head_.store(0, std::memory_order_release);
|
{
|
||||||
tail_.store(0, std::memory_order_release);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
// Keep sequence numbers monotonic so cursors created before clear()
|
||||||
|
// cannot alias newly published slots after the reset.
|
||||||
|
tail_ = head_;
|
||||||
|
}
|
||||||
|
condition_.notify_all();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::vector<T> buffer_;
|
mutable std::vector<T> buffer_;
|
||||||
const size_t capacity_;
|
const size_t capacity_;
|
||||||
|
mutable std::mutex mutex_;
|
||||||
|
mutable std::condition_variable condition_;
|
||||||
|
size_t head_{0}; // 单调写序号；clear() 仅推进 tail_，避免旧游标 ABA。
|
||||||
|
size_t tail_{0}; // 当前仍保留的最旧序号，同时也是 pop() 的共享读指针。
|
||||||
|
};
|
||||||
|
|
||||||
std::atomic<size_t> head_; // 共享写指针
|
// 线程安全的多消费者广播缓冲区。缓冲区只保存不可变共享对象，消费者通过各自
|
||||||
std::atomic<size_t> tail_; // 共享读指针（仅用于 SPSC 模式）
|
// 的 Cursor 独立前进；慢消费者被覆盖的数据会累计到 Cursor::dropped_count。
|
||||||
|
// Cursor 是单线程所有权对象，不可由多个线程同时读写；每个消费者应创建自己的 Cursor。
|
||||||
|
template<typename T>
|
||||||
|
class BroadcastFrameRing {
|
||||||
|
public:
|
||||||
|
using ValuePtr = std::shared_ptr<const T>;
|
||||||
|
|
||||||
|
enum class StartPosition {
|
||||||
|
NEXT_PUBLISHED,
|
||||||
|
OLDEST_AVAILABLE,
|
||||||
|
LATEST_AVAILABLE
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Cursor {
|
||||||
|
uint64_t generation{0};
|
||||||
|
uint64_t next_sequence{0};
|
||||||
|
uint64_t dropped_count{0};
|
||||||
|
StartPosition start_position{StartPosition::NEXT_PUBLISHED};
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool generation_changed{false};
|
||||||
|
uint64_t reported_dropped_count{0};
|
||||||
|
friend class BroadcastFrameRing<T>;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct ReadResult {
|
||||||
|
ValuePtr value;
|
||||||
|
uint64_t generation{0};
|
||||||
|
uint64_t sequence{0};
|
||||||
|
uint64_t dropped_count{0};
|
||||||
|
uint64_t dropped_since_last_read{0};
|
||||||
|
bool generation_changed{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Stats {
|
||||||
|
size_t capacity{0};
|
||||||
|
size_t size{0};
|
||||||
|
uint64_t generation{0};
|
||||||
|
uint64_t next_sequence{0};
|
||||||
|
uint64_t dropped_count{0};
|
||||||
|
bool closed{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
explicit BroadcastFrameRing(const size_t capacity)
|
||||||
|
: capacity_(capacity) {
|
||||||
|
if (capacity_ == 0) {
|
||||||
|
throw std::invalid_argument("BroadcastFrameRing capacity must be greater than zero");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
BroadcastFrameRing(const BroadcastFrameRing&) = delete;
|
||||||
|
BroadcastFrameRing& operator=(const BroadcastFrameRing&) = delete;
|
||||||
|
|
||||||
|
Cursor makeCursor(const StartPosition start_position = StartPosition::NEXT_PUBLISHED) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
Cursor cursor;
|
||||||
|
cursor.generation = generation_;
|
||||||
|
cursor.start_position = start_position;
|
||||||
|
cursor.next_sequence = startSequenceLocked_(start_position);
|
||||||
|
return cursor;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<uint64_t> publish(ValuePtr value) {
|
||||||
|
if (!value) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<uint64_t> published_sequence;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (closed_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t sequence = next_sequence_++;
|
||||||
|
if (entries_.size() == capacity_) {
|
||||||
|
entries_.pop_front();
|
||||||
|
++dropped_count_;
|
||||||
|
}
|
||||||
|
entries_.push_back(Entry{generation_, sequence, std::move(value)});
|
||||||
|
published_sequence = sequence;
|
||||||
|
}
|
||||||
|
condition_.notify_all();
|
||||||
|
return published_sequence;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<ReadResult> tryRead(Cursor& cursor) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return tryReadLocked_(cursor);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Low-latency consumers can use this before reading to abandon an excessive
|
||||||
|
// backlog atomically. When the number of currently readable entries exceeds
|
||||||
|
// maximum_pending_frames, every pending entry is discarded and the cursor is
|
||||||
|
// advanced to the next sequence that will be published. Frames already
|
||||||
|
// overwritten by the ring and frames actively discarded here are both
|
||||||
|
// reflected in Cursor::dropped_count; the next successful read reports their
|
||||||
|
// sum through ReadResult::dropped_since_last_read.
|
||||||
|
uint64_t discardPendingIfExceeds(
|
||||||
|
Cursor& cursor,
|
||||||
|
const size_t maximum_pending_frames) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
synchronizeCursorGenerationLocked_(cursor);
|
||||||
|
|
||||||
|
if (!entries_.empty()) {
|
||||||
|
const uint64_t oldest_sequence = entries_.front().sequence;
|
||||||
|
if (cursor.next_sequence < oldest_sequence) {
|
||||||
|
cursor.dropped_count += oldest_sequence - cursor.next_sequence;
|
||||||
|
cursor.next_sequence = oldest_sequence;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t pending =
|
||||||
|
cursor.next_sequence < next_sequence_
|
||||||
|
? next_sequence_ - cursor.next_sequence
|
||||||
|
: 0;
|
||||||
|
if (pending <= static_cast<uint64_t>(maximum_pending_frames)) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
cursor.next_sequence = next_sequence_;
|
||||||
|
cursor.dropped_count += pending;
|
||||||
|
return pending;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<class Rep, class Period>
|
||||||
|
std::optional<ReadResult> waitRead(
|
||||||
|
Cursor& cursor,
|
||||||
|
const std::chrono::duration<Rep, Period>& timeout) const {
|
||||||
|
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||||
|
std::unique_lock<std::mutex> lock(mutex_);
|
||||||
|
while (true) {
|
||||||
|
if (auto result = tryReadLocked_(cursor)) {
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
if (closed_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
if (condition_.wait_until(lock, deadline) == std::cv_status::timeout) {
|
||||||
|
return tryReadLocked_(cursor);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 开始一个新的发布代次。旧 Cursor 会在下一次成功读取时收到
|
||||||
|
// generation_changed=true，序号从 0 重新开始。
|
||||||
|
uint64_t reset() {
|
||||||
|
uint64_t generation = 0;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
entries_.clear();
|
||||||
|
++generation_;
|
||||||
|
next_sequence_ = 0;
|
||||||
|
closed_ = false;
|
||||||
|
generation = generation_;
|
||||||
|
}
|
||||||
|
condition_.notify_all();
|
||||||
|
return generation;
|
||||||
|
}
|
||||||
|
|
||||||
|
void close() {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
closed_ = true;
|
||||||
|
}
|
||||||
|
condition_.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool closed() const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return closed_;
|
||||||
|
}
|
||||||
|
|
||||||
|
Stats stats() const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return Stats{capacity_, entries_.size(), generation_, next_sequence_, dropped_count_, closed_};
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Entry {
|
||||||
|
uint64_t generation;
|
||||||
|
uint64_t sequence;
|
||||||
|
ValuePtr value;
|
||||||
|
};
|
||||||
|
|
||||||
|
uint64_t startSequenceLocked_(const StartPosition start_position) const {
|
||||||
|
if (entries_.empty()) {
|
||||||
|
return next_sequence_;
|
||||||
|
}
|
||||||
|
switch (start_position) {
|
||||||
|
case StartPosition::OLDEST_AVAILABLE:
|
||||||
|
return entries_.front().sequence;
|
||||||
|
case StartPosition::LATEST_AVAILABLE:
|
||||||
|
return entries_.back().sequence;
|
||||||
|
case StartPosition::NEXT_PUBLISHED:
|
||||||
|
default:
|
||||||
|
return next_sequence_;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void synchronizeCursorGenerationLocked_(Cursor& cursor) const {
|
||||||
|
if (cursor.generation == generation_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
cursor.generation = generation_;
|
||||||
|
cursor.next_sequence = startSequenceLocked_(cursor.start_position);
|
||||||
|
cursor.generation_changed = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<ReadResult> tryReadLocked_(Cursor& cursor) const {
|
||||||
|
synchronizeCursorGenerationLocked_(cursor);
|
||||||
|
if (entries_.empty()) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t oldest_sequence = entries_.front().sequence;
|
||||||
|
if (cursor.next_sequence < oldest_sequence) {
|
||||||
|
cursor.dropped_count += oldest_sequence - cursor.next_sequence;
|
||||||
|
cursor.next_sequence = oldest_sequence;
|
||||||
|
}
|
||||||
|
if (cursor.next_sequence >= next_sequence_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
const size_t index = static_cast<size_t>(cursor.next_sequence - oldest_sequence);
|
||||||
|
if (index >= entries_.size()) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
const Entry& entry = entries_[index];
|
||||||
|
++cursor.next_sequence;
|
||||||
|
const uint64_t dropped_since_last = cursor.dropped_count - cursor.reported_dropped_count;
|
||||||
|
cursor.reported_dropped_count = cursor.dropped_count;
|
||||||
|
|
||||||
|
ReadResult result;
|
||||||
|
result.value = entry.value;
|
||||||
|
result.generation = entry.generation;
|
||||||
|
result.sequence = entry.sequence;
|
||||||
|
result.dropped_count = cursor.dropped_count;
|
||||||
|
result.dropped_since_last_read = dropped_since_last;
|
||||||
|
result.generation_changed = cursor.generation_changed;
|
||||||
|
cursor.generation_changed = false;
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
const size_t capacity_;
|
||||||
|
mutable std::mutex mutex_;
|
||||||
|
mutable std::condition_variable condition_;
|
||||||
|
std::deque<Entry> entries_;
|
||||||
|
uint64_t generation_{1};
|
||||||
|
uint64_t next_sequence_{0};
|
||||||
|
uint64_t dropped_count_{0};
|
||||||
|
bool closed_{false};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
244
cmvr-es/common/media/media_frame.h
Normal file
244
cmvr-es/common/media/media_frame.h
Normal file
@ -0,0 +1,244 @@
|
|||||||
|
#ifndef CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H
|
||||||
|
#define CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <memory>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace cmvr::media {
|
||||||
|
|
||||||
|
enum class MediaKind : uint8_t {
|
||||||
|
UNKNOWN = 0,
|
||||||
|
VIDEO = 1,
|
||||||
|
AUDIO = 2
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class Codec : uint8_t {
|
||||||
|
UNKNOWN = 0,
|
||||||
|
H264 = 1,
|
||||||
|
H265 = 2,
|
||||||
|
OPUS = 3,
|
||||||
|
PCM_S16LE = 4,
|
||||||
|
AAC = 5
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class PayloadFormat : uint8_t {
|
||||||
|
UNKNOWN = 0,
|
||||||
|
ANNEX_B = 1,
|
||||||
|
AVCC = 2,
|
||||||
|
RAW = 3,
|
||||||
|
OPUS_PACKET = 4,
|
||||||
|
AAC_ADTS = 5
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Rational {
|
||||||
|
int32_t numerator{0};
|
||||||
|
int32_t denominator{1};
|
||||||
|
|
||||||
|
constexpr bool valid() const noexcept {
|
||||||
|
return numerator > 0 && denominator > 0;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
constexpr bool operator==(const Rational lhs, const Rational rhs) noexcept {
|
||||||
|
return lhs.numerator == rhs.numerator && lhs.denominator == rhs.denominator;
|
||||||
|
}
|
||||||
|
|
||||||
|
constexpr bool operator!=(const Rational lhs, const Rational rhs) noexcept {
|
||||||
|
return !(lhs == rhs);
|
||||||
|
}
|
||||||
|
|
||||||
|
// A TrackDescriptor is immutable after construction. Reconfiguration is represented by
|
||||||
|
// publishing a new descriptor with a larger generation and attaching it to later frames.
|
||||||
|
class TrackDescriptor final {
|
||||||
|
public:
|
||||||
|
struct Config {
|
||||||
|
std::string id;
|
||||||
|
std::string source_id;
|
||||||
|
MediaKind kind{MediaKind::UNKNOWN};
|
||||||
|
Codec codec{Codec::UNKNOWN};
|
||||||
|
PayloadFormat payload_format{PayloadFormat::UNKNOWN};
|
||||||
|
Rational time_base{};
|
||||||
|
uint32_t width{0};
|
||||||
|
uint32_t height{0};
|
||||||
|
uint32_t sample_rate{0};
|
||||||
|
uint32_t channels{0};
|
||||||
|
uint32_t nominal_rate{0};
|
||||||
|
float fx{0.0F};
|
||||||
|
float fy{0.0F};
|
||||||
|
float cx{0.0F};
|
||||||
|
float cy{0.0F};
|
||||||
|
std::vector<float> distortion;
|
||||||
|
uint64_t generation{1};
|
||||||
|
std::vector<uint8_t> codec_config;
|
||||||
|
};
|
||||||
|
|
||||||
|
explicit TrackDescriptor(Config config)
|
||||||
|
: id(std::move(config.id)),
|
||||||
|
source_id(std::move(config.source_id)),
|
||||||
|
kind(config.kind),
|
||||||
|
codec(config.codec),
|
||||||
|
payload_format(config.payload_format),
|
||||||
|
time_base(config.time_base),
|
||||||
|
width(config.width),
|
||||||
|
height(config.height),
|
||||||
|
sample_rate(config.sample_rate),
|
||||||
|
channels(config.channels),
|
||||||
|
nominal_rate(config.nominal_rate),
|
||||||
|
fx(config.fx),
|
||||||
|
fy(config.fy),
|
||||||
|
cx(config.cx),
|
||||||
|
cy(config.cy),
|
||||||
|
distortion(std::move(config.distortion)),
|
||||||
|
generation(config.generation),
|
||||||
|
codec_config(std::move(config.codec_config)) {
|
||||||
|
if (id.empty()) {
|
||||||
|
throw std::invalid_argument("TrackDescriptor id must not be empty");
|
||||||
|
}
|
||||||
|
if (source_id.empty()) {
|
||||||
|
throw std::invalid_argument("TrackDescriptor source_id must not be empty");
|
||||||
|
}
|
||||||
|
if (kind == MediaKind::UNKNOWN) {
|
||||||
|
throw std::invalid_argument("TrackDescriptor kind must not be UNKNOWN");
|
||||||
|
}
|
||||||
|
if (!time_base.valid()) {
|
||||||
|
throw std::invalid_argument("TrackDescriptor time_base is invalid");
|
||||||
|
}
|
||||||
|
if (generation == 0) {
|
||||||
|
throw std::invalid_argument("TrackDescriptor generation must be greater than zero");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string id;
|
||||||
|
const std::string source_id;
|
||||||
|
const MediaKind kind;
|
||||||
|
const Codec codec;
|
||||||
|
const PayloadFormat payload_format;
|
||||||
|
const Rational time_base;
|
||||||
|
const uint32_t width;
|
||||||
|
const uint32_t height;
|
||||||
|
const uint32_t sample_rate;
|
||||||
|
const uint32_t channels;
|
||||||
|
const uint32_t nominal_rate;
|
||||||
|
const float fx;
|
||||||
|
const float fy;
|
||||||
|
const float cx;
|
||||||
|
const float cy;
|
||||||
|
const std::vector<float> distortion;
|
||||||
|
const uint64_t generation;
|
||||||
|
const std::vector<uint8_t> codec_config;
|
||||||
|
};
|
||||||
|
|
||||||
|
using TrackDescriptorPtr = std::shared_ptr<const TrackDescriptor>;
|
||||||
|
|
||||||
|
inline bool equivalentTrackDescriptor(
|
||||||
|
const TrackDescriptor& lhs,
|
||||||
|
const TrackDescriptor& rhs) noexcept {
|
||||||
|
return lhs.id == rhs.id &&
|
||||||
|
lhs.source_id == rhs.source_id &&
|
||||||
|
lhs.kind == rhs.kind &&
|
||||||
|
lhs.codec == rhs.codec &&
|
||||||
|
lhs.payload_format == rhs.payload_format &&
|
||||||
|
lhs.time_base == rhs.time_base &&
|
||||||
|
lhs.width == rhs.width &&
|
||||||
|
lhs.height == rhs.height &&
|
||||||
|
lhs.sample_rate == rhs.sample_rate &&
|
||||||
|
lhs.channels == rhs.channels &&
|
||||||
|
lhs.nominal_rate == rhs.nominal_rate &&
|
||||||
|
lhs.fx == rhs.fx &&
|
||||||
|
lhs.fy == rhs.fy &&
|
||||||
|
lhs.cx == rhs.cx &&
|
||||||
|
lhs.cy == rhs.cy &&
|
||||||
|
lhs.distortion == rhs.distortion &&
|
||||||
|
lhs.generation == rhs.generation &&
|
||||||
|
lhs.codec_config == rhs.codec_config;
|
||||||
|
}
|
||||||
|
|
||||||
|
// MediaFrame and its payload are immutable and therefore safe to share across all protocol
|
||||||
|
// adapters and consumers without copying. PTS/DTS use TrackDescriptor::time_base;
|
||||||
|
// capture_time_ns is monotonic for pacing, while capture_utc_ns is optional wall time.
|
||||||
|
class MediaFrame final {
|
||||||
|
public:
|
||||||
|
using Payload = std::vector<uint8_t>;
|
||||||
|
using PayloadPtr = std::shared_ptr<const Payload>;
|
||||||
|
|
||||||
|
struct Config {
|
||||||
|
TrackDescriptorPtr descriptor;
|
||||||
|
Payload payload;
|
||||||
|
uint64_t sequence{0};
|
||||||
|
// Opaque producer-native timing/counter values. Their units and epoch
|
||||||
|
// are source-defined; zero means that the source did not provide them.
|
||||||
|
uint64_t source_timestamp{0};
|
||||||
|
uint64_t source_frame_number{0};
|
||||||
|
int64_t pts{0};
|
||||||
|
int64_t dts{0};
|
||||||
|
int64_t duration{0};
|
||||||
|
uint64_t capture_time_ns{0};
|
||||||
|
int64_t capture_utc_ns{0};
|
||||||
|
bool key_frame{false};
|
||||||
|
bool discontinuity{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
explicit MediaFrame(Config config)
|
||||||
|
: descriptor(std::move(config.descriptor)),
|
||||||
|
payload(std::make_shared<const Payload>(std::move(config.payload))),
|
||||||
|
sequence(config.sequence),
|
||||||
|
source_timestamp(config.source_timestamp),
|
||||||
|
source_frame_number(config.source_frame_number),
|
||||||
|
pts(config.pts),
|
||||||
|
dts(config.dts),
|
||||||
|
duration(config.duration),
|
||||||
|
capture_time_ns(config.capture_time_ns),
|
||||||
|
capture_utc_ns(config.capture_utc_ns),
|
||||||
|
key_frame(config.key_frame),
|
||||||
|
discontinuity(config.discontinuity) {
|
||||||
|
if (!descriptor) {
|
||||||
|
throw std::invalid_argument("MediaFrame descriptor must not be null");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint8_t* data() const noexcept {
|
||||||
|
return payload->empty() ? nullptr : payload->data();
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t size() const noexcept {
|
||||||
|
return payload->size();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool empty() const noexcept {
|
||||||
|
return payload->empty();
|
||||||
|
}
|
||||||
|
|
||||||
|
const TrackDescriptorPtr descriptor;
|
||||||
|
const PayloadPtr payload;
|
||||||
|
const uint64_t sequence;
|
||||||
|
const uint64_t source_timestamp;
|
||||||
|
const uint64_t source_frame_number;
|
||||||
|
const int64_t pts;
|
||||||
|
const int64_t dts;
|
||||||
|
const int64_t duration;
|
||||||
|
const uint64_t capture_time_ns;
|
||||||
|
const int64_t capture_utc_ns;
|
||||||
|
const bool key_frame;
|
||||||
|
const bool discontinuity;
|
||||||
|
};
|
||||||
|
|
||||||
|
using MediaFramePtr = std::shared_ptr<const MediaFrame>;
|
||||||
|
|
||||||
|
inline TrackDescriptorPtr makeTrackDescriptor(TrackDescriptor::Config config) {
|
||||||
|
return std::make_shared<const TrackDescriptor>(std::move(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
inline MediaFramePtr makeMediaFrame(MediaFrame::Config config) {
|
||||||
|
return std::make_shared<const MediaFrame>(std::move(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::media
|
||||||
|
|
||||||
|
#endif // CMVR_ES_COMMON_MEDIA_MEDIA_FRAME_H
|
||||||
420
cmvr-es/common/types/agv/agv_types.h
Normal file
420
cmvr-es/common/types/agv/agv_types.h
Normal file
@ -0,0 +1,420 @@
|
|||||||
|
#ifndef CMVR_ES_AGV_TYPES_H
|
||||||
|
#define CMVR_ES_AGV_TYPES_H
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/types/geometry_types.h"
|
||||||
|
|
||||||
|
namespace cmvr::device {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 通用命令/结果错误类别。
|
||||||
|
*
|
||||||
|
* 这些枚举描述框架层面的通用结果。厂商或控制器特有错误码应由具体
|
||||||
|
* AGV 实现转换，或保存在该实现私有的适配参数/细节中。
|
||||||
|
*/
|
||||||
|
enum class AgvErrorCode {
|
||||||
|
OK = 0,
|
||||||
|
NotConnected,
|
||||||
|
AlreadyConnected,
|
||||||
|
ConnectionFailed,
|
||||||
|
Timeout,
|
||||||
|
InvalidArgument,
|
||||||
|
LocalizationLost,
|
||||||
|
MapNotLoaded,
|
||||||
|
TaskRejected,
|
||||||
|
TaskFailed,
|
||||||
|
TaskCanceled,
|
||||||
|
CommandFailed,
|
||||||
|
EmergencyStopped,
|
||||||
|
Fault,
|
||||||
|
UnsupportedCommand,
|
||||||
|
UnknownError
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 命令的标准返回值。
|
||||||
|
*/
|
||||||
|
struct AgvResult {
|
||||||
|
AgvErrorCode code{AgvErrorCode::OK};
|
||||||
|
std::string message{"OK"};
|
||||||
|
|
||||||
|
bool ok() const { return code == AgvErrorCode::OK; }
|
||||||
|
static AgvResult success() { return {AgvErrorCode::OK, "OK"}; }
|
||||||
|
static AgvResult failure(AgvErrorCode c, const std::string& msg) { return {c, msg}; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 粗粒度运行模式。
|
||||||
|
*/
|
||||||
|
enum class AgvMode {
|
||||||
|
Unknown = 0,
|
||||||
|
Disconnected,
|
||||||
|
Idle,
|
||||||
|
Manual,
|
||||||
|
Auto,
|
||||||
|
Charging,
|
||||||
|
Paused,
|
||||||
|
Stopped,
|
||||||
|
Fault,
|
||||||
|
EmergencyStop
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 当前跟踪的导航任务状态。
|
||||||
|
*/
|
||||||
|
enum class AgvTaskState {
|
||||||
|
None = 0,
|
||||||
|
Waiting,
|
||||||
|
Running,
|
||||||
|
Paused,
|
||||||
|
Completed,
|
||||||
|
Failed,
|
||||||
|
Canceled
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 当前跟踪的导航任务类型。
|
||||||
|
*/
|
||||||
|
enum class AgvTaskType {
|
||||||
|
None = 0,
|
||||||
|
NavigateToPose,
|
||||||
|
NavigateToStation,
|
||||||
|
FollowPath,
|
||||||
|
Dock,
|
||||||
|
Charge,
|
||||||
|
Custom
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 车体坐标系下的平面速度。
|
||||||
|
*
|
||||||
|
* 线速度单位为米/秒，角速度单位为弧度/秒。
|
||||||
|
*/
|
||||||
|
struct AgvVelocity {
|
||||||
|
double vx{0.0};
|
||||||
|
double vy{0.0};
|
||||||
|
double wz{0.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 导航通用运动约束和执行选项。
|
||||||
|
*
|
||||||
|
* 除非具体实现另有说明，数值限制为 0 表示使用设备或控制器默认值。
|
||||||
|
*/
|
||||||
|
struct AgvMotionOptions {
|
||||||
|
double max_speed{0.0};
|
||||||
|
double max_angular_speed{0.0};
|
||||||
|
double max_acceleration{0.0};
|
||||||
|
double max_angular_acceleration{0.0};
|
||||||
|
double reach_distance{0.0};
|
||||||
|
double reach_angle{0.0};
|
||||||
|
double speed_ratio{1.0};
|
||||||
|
bool asynchronous{true};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 适配器可选的实现特定参数。
|
||||||
|
*
|
||||||
|
* 该结构用于避免抽象接口绑定某一个控制器协议。具体 AGV 驱动可以按需
|
||||||
|
* 解释操作名、特殊运动模式、设备特定标志等键值。
|
||||||
|
*/
|
||||||
|
struct AgvAdapterParams {
|
||||||
|
std::unordered_map<std::string, std::string> values;
|
||||||
|
|
||||||
|
bool empty() const { return values.empty(); }
|
||||||
|
|
||||||
|
std::optional<std::string> getString(const std::string& key) const
|
||||||
|
{
|
||||||
|
const auto it = values.find(key);
|
||||||
|
if (it == values.end()) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<double> getDouble(const std::string& key) const
|
||||||
|
{
|
||||||
|
const auto value = getString(key);
|
||||||
|
if (!value) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
return std::stod(*value);
|
||||||
|
} catch (...) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<bool> getBool(const std::string& key) const
|
||||||
|
{
|
||||||
|
const auto value = getString(key);
|
||||||
|
if (!value) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
if (*value == "1" || *value == "true" || *value == "yes" || *value == "on") {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (*value == "0" || *value == "false" || *value == "no" || *value == "off") {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 作为 AgvRuntimeState 一部分暴露的电池信息。
|
||||||
|
*/
|
||||||
|
struct AgvBatteryState {
|
||||||
|
double percentage{0.0};
|
||||||
|
double voltage{0.0};
|
||||||
|
double current{0.0};
|
||||||
|
double temperature{0.0};
|
||||||
|
bool charging{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 当前运行状态快照。
|
||||||
|
*
|
||||||
|
* 这是 AGV 设备的主要状态查询对象。抽象接口中应避免派生出的便利
|
||||||
|
* getter；调用方可直接从该快照读取字段。
|
||||||
|
*/
|
||||||
|
struct AgvRuntimeState {
|
||||||
|
double timestamp{0.0};
|
||||||
|
AgvMode mode{AgvMode::Unknown};
|
||||||
|
bool connected{false};
|
||||||
|
bool localized{false};
|
||||||
|
bool moving{false};
|
||||||
|
bool fault{false};
|
||||||
|
bool emergency_stopped{false};
|
||||||
|
math::Pose2d pose{};
|
||||||
|
AgvVelocity velocity{};
|
||||||
|
AgvBatteryState battery{};
|
||||||
|
std::string current_map;
|
||||||
|
std::string current_station;
|
||||||
|
std::string last_error;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 抽象层可见的地图站点/路径点。
|
||||||
|
*/
|
||||||
|
struct AgvStation {
|
||||||
|
std::string id;
|
||||||
|
std::string type;
|
||||||
|
math::Pose2d pose{};
|
||||||
|
std::string description;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 显式导航路径中的一段站点到站点路径。
|
||||||
|
*/
|
||||||
|
struct AgvPathSegment {
|
||||||
|
std::string source_station;
|
||||||
|
std::string target_station;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief AGV 扫图过程中产生的数据文件。
|
||||||
|
*
|
||||||
|
* content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
|
||||||
|
*/
|
||||||
|
struct AgvMappingDataFile {
|
||||||
|
std::string name;
|
||||||
|
std::string content;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 从控制器增量获取的扫图数据批次。
|
||||||
|
*/
|
||||||
|
struct AgvMappingData {
|
||||||
|
int start_index{0};
|
||||||
|
int next_index{0};
|
||||||
|
std::vector<AgvMappingDataFile> files;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 上位机请求的统一地图维度。
|
||||||
|
*
|
||||||
|
* 该枚举只表示上位机希望得到 2D、3D 或两者都要；不表示厂商文件格式。
|
||||||
|
* 厂商原始地图必须由具体 AGV 驱动转换为下面的统一地图结构。
|
||||||
|
*/
|
||||||
|
enum class AgvMapDimension {
|
||||||
|
Unspecified = 0,
|
||||||
|
Map2D,
|
||||||
|
Map3D,
|
||||||
|
Map2DAnd3D
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 地图流中的更新类型。
|
||||||
|
*/
|
||||||
|
enum class AgvMapUpdateType {
|
||||||
|
Unspecified = 0,
|
||||||
|
Snapshot,
|
||||||
|
Incremental,
|
||||||
|
Reset
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一语义地图对象类型。
|
||||||
|
*/
|
||||||
|
enum class AgvMapObjectType {
|
||||||
|
Unspecified = 0,
|
||||||
|
Station,
|
||||||
|
Line,
|
||||||
|
Area,
|
||||||
|
QrTag,
|
||||||
|
Reflector,
|
||||||
|
BinLocation,
|
||||||
|
ExternalDevice
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 地图坐标系下的三维点，单位：米。
|
||||||
|
*/
|
||||||
|
struct AgvMapPoint3D {
|
||||||
|
double x{0.0};
|
||||||
|
double y{0.0};
|
||||||
|
double z{0.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一语义对象。几何点均使用地图坐标系，单位：米。
|
||||||
|
*/
|
||||||
|
struct AgvMapObject {
|
||||||
|
std::string id;
|
||||||
|
AgvMapObjectType type{AgvMapObjectType::Unspecified};
|
||||||
|
std::vector<AgvMapPoint3D> points;
|
||||||
|
double heading{0.0};
|
||||||
|
std::unordered_map<std::string, std::string> properties;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一 2D 地图。
|
||||||
|
*
|
||||||
|
* data 采用行优先顺序，取值约定为 -1 未知、0 空闲、100 占据。
|
||||||
|
* 当厂商地图只提供矢量/语义元素时，data 可以为空，objects 仍然有效。
|
||||||
|
*/
|
||||||
|
struct AgvUnifiedMap2D {
|
||||||
|
std::string frame_id{"map"};
|
||||||
|
double timestamp{0.0};
|
||||||
|
double resolution{0.0};
|
||||||
|
std::uint32_t width{0};
|
||||||
|
std::uint32_t height{0};
|
||||||
|
math::Pose2d origin{};
|
||||||
|
std::vector<std::int32_t> data;
|
||||||
|
std::vector<AgvMapObject> objects;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一 3D 点样本，坐标单位：米。
|
||||||
|
*/
|
||||||
|
struct AgvMapPointSample3D {
|
||||||
|
double x{0.0};
|
||||||
|
double y{0.0};
|
||||||
|
double z{0.0};
|
||||||
|
float intensity{0.0F};
|
||||||
|
std::uint32_t ring{0};
|
||||||
|
double time_offset{0.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一 3D 占据体素。
|
||||||
|
*/
|
||||||
|
struct AgvMapVoxel3D {
|
||||||
|
std::int32_t x{0};
|
||||||
|
std::int32_t y{0};
|
||||||
|
std::int32_t z{0};
|
||||||
|
float probability{-1.0F};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一 3D 平面特征。
|
||||||
|
*/
|
||||||
|
struct AgvMapPlane3D {
|
||||||
|
AgvMapPoint3D center;
|
||||||
|
AgvMapPoint3D normal;
|
||||||
|
double d{0.0};
|
||||||
|
double radius{0.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一 3D 地图。
|
||||||
|
*/
|
||||||
|
struct AgvUnifiedMap3D {
|
||||||
|
std::string frame_id{"map"};
|
||||||
|
double timestamp{0.0};
|
||||||
|
double voxel_resolution{0.0};
|
||||||
|
std::vector<AgvMapPointSample3D> points;
|
||||||
|
std::vector<AgvMapVoxel3D> voxels;
|
||||||
|
std::vector<AgvMapPlane3D> planes;
|
||||||
|
std::vector<AgvMapObject> objects;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 地图流读取参数。
|
||||||
|
*/
|
||||||
|
struct AgvMapStreamOptions {
|
||||||
|
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||||
|
std::string map_name;
|
||||||
|
std::string resume_token;
|
||||||
|
bool snapshot{true};
|
||||||
|
bool incremental{false};
|
||||||
|
int max_chunk_bytes{0};
|
||||||
|
int wait_timeout_ms{1000};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 建图/扫图启动参数。
|
||||||
|
*/
|
||||||
|
struct AgvMappingOptions {
|
||||||
|
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||||
|
std::string map_name;
|
||||||
|
bool real_time{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 统一地图流中的单条更新。
|
||||||
|
*/
|
||||||
|
struct AgvUnifiedMapUpdate {
|
||||||
|
std::string map_id;
|
||||||
|
std::string session_id;
|
||||||
|
std::uint64_t sequence{0};
|
||||||
|
std::string resume_token;
|
||||||
|
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||||
|
AgvMapUpdateType update_type{AgvMapUpdateType::Unspecified};
|
||||||
|
std::string frame_id{"map"};
|
||||||
|
double timestamp{0.0};
|
||||||
|
bool snapshot_begin{false};
|
||||||
|
bool snapshot_end{false};
|
||||||
|
std::uint32_t chunk_index{0};
|
||||||
|
std::uint32_t chunk_count{0};
|
||||||
|
std::optional<AgvUnifiedMap2D> map_2d;
|
||||||
|
std::optional<AgvUnifiedMap3D> map_3d;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 当前导航任务状态。
|
||||||
|
*
|
||||||
|
* 该状态独立于 AgvRuntimeState，因为导航任务可能处于排队、暂停、
|
||||||
|
* 完成或失败状态，而车辆本体仍然保持连接并处于正常状态。
|
||||||
|
*/
|
||||||
|
struct AgvNavigationStatus {
|
||||||
|
AgvTaskState state{AgvTaskState::None};
|
||||||
|
AgvTaskType type{AgvTaskType::None};
|
||||||
|
double progress{0.0};
|
||||||
|
std::string message;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 兼容旧 AGV 状态 API 命名的别名。
|
||||||
|
*/
|
||||||
|
using AGVState = AgvRuntimeState;
|
||||||
|
|
||||||
|
} // namespace cmvr::device
|
||||||
|
|
||||||
|
#endif // CMVR_ES_AGV_TYPES_H
|
||||||
@ -49,7 +49,7 @@ namespace cmvr::math {
|
|||||||
typedef struct {
|
typedef struct {
|
||||||
double x; //* unit: m
|
double x; //* unit: m
|
||||||
double y;
|
double y;
|
||||||
double theta;
|
double theta; //* unit: rad
|
||||||
} Pose2d;
|
} Pose2d;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
130
cmvr-es/config/README.md
Normal file
130
cmvr-es/config/README.md
Normal file
@ -0,0 +1,130 @@
|
|||||||
|
# Config 模块开发指南
|
||||||
|
|
||||||
|
`config/` 保存 CMVR-ES 的默认运行配置。配置格式是 Protobuf TextFormat，Schema 位于 [`../../protos/cmvr/config/`](../../protos/cmvr/config/)。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 配置树
|
||||||
|
|
||||||
|
```text
|
||||||
|
cmvr_es.pb.txt
|
||||||
|
├── logger/logger.pb.txt
|
||||||
|
├── manager/device_manager.pb.txt
|
||||||
|
│ └── devices/<category>/*.pb.txt
|
||||||
|
└── manager/task_manager.pb.txt
|
||||||
|
└── tasks/<task>/*.pb.txt
|
||||||
|
```
|
||||||
|
|
||||||
|
入口文件：
|
||||||
|
|
||||||
|
- [`cmvr_es.pb.txt`](cmvr_es.pb.txt)
|
||||||
|
- [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt)
|
||||||
|
- [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt)
|
||||||
|
|
||||||
|
## 路径规则
|
||||||
|
|
||||||
|
无参数运行时，程序读取：
|
||||||
|
|
||||||
|
```text
|
||||||
|
<cmvr_es 可执行文件所在目录>/config/cmvr_es.pb.txt
|
||||||
|
```
|
||||||
|
|
||||||
|
安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt
|
||||||
|
```
|
||||||
|
|
||||||
|
设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。
|
||||||
|
|
||||||
|
日志配置中的相对 `directory` 以可执行文件目录解析，不以配置根解析。
|
||||||
|
|
||||||
|
## 新增设备配置
|
||||||
|
|
||||||
|
增加同类设备后端时：
|
||||||
|
|
||||||
|
1. 在 `protos/cmvr/config/<category>_config/` 增加后端 message；
|
||||||
|
2. 在类别设备 message 的 `oneof backend` 中增加字段；
|
||||||
|
3. 在 `devices/<category>/` 的 `.pb.txt` 中增加实例；
|
||||||
|
4. 实例外层 `id` 必须唯一；
|
||||||
|
5. 在 [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 增加相同 `id`、正确 `type` 和配置路径；
|
||||||
|
6. 开发默认保持 `enable: false`；
|
||||||
|
7. 同步类别 factory 和 CMake；
|
||||||
|
8. 在无硬件环境验证关闭状态，在真机环境单独开启。
|
||||||
|
|
||||||
|
设备集合中的 ID 与 DeviceManager 条目 ID 不一致时，工厂会拒绝创建。
|
||||||
|
|
||||||
|
## 新增任务配置
|
||||||
|
|
||||||
|
1. 在 `protos/cmvr/config/` 增加任务配置和 root message；
|
||||||
|
2. 在 `tasks/<task_name>/` 增加默认 `.pb.txt`；
|
||||||
|
3. 在 `task_manager.pb.txt` 增加唯一任务 ID；
|
||||||
|
4. 配置正确的 `TaskType` 和 `TaskRunMode`；
|
||||||
|
5. 周期任务设置大于零的 `control_period_s`；
|
||||||
|
6. 服务任务使用 `TASK_RUN_MODE_BLOCKING_SERVICE`；
|
||||||
|
7. 默认关闭依赖网络、证书或硬件的新任务。
|
||||||
|
|
||||||
|
任务实现流程见 [`../task/README.md`](../task/README.md)。
|
||||||
|
|
||||||
|
## 默认值与校验
|
||||||
|
|
||||||
|
- 不依赖 proto3 数值零值表达危险的生产默认值；
|
||||||
|
- timeout、队列大小、帧大小和周期应在代码中校验；
|
||||||
|
- 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制；
|
||||||
|
- 设备端口、坐标系、速度和单位写入注释；
|
||||||
|
- `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关；
|
||||||
|
- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启；
|
||||||
|
- QUIC 零媒体轨道是合法配置。
|
||||||
|
|
||||||
|
### gRPC 相机实时流
|
||||||
|
|
||||||
|
[`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt)
|
||||||
|
中的两个低延迟参数仅作用于 gRPC RGB 编码流，不改变机械臂、AGV 等控制 RPC：
|
||||||
|
|
||||||
|
- `camera_stream_max_pending_frames`：单个客户端允许的待发送帧数，超过后清空该
|
||||||
|
客户端积压；默认 2；
|
||||||
|
- `camera_stream_max_frame_age_ms`：从设备回调进入边缘系统起计算的最大帧龄，
|
||||||
|
超过后不再发送；默认 250 ms。
|
||||||
|
|
||||||
|
两个字段填 0 或旧配置未包含字段时使用默认值。丢弃 H.264/H.265 帧后服务会请求
|
||||||
|
IDR 并等待关键帧恢复。如果现场采集、编码本身稳定超过 250 ms，应根据日志中的
|
||||||
|
`age_ms` 调高帧龄阈值，而不是增大环形队列。
|
||||||
|
|
||||||
|
新二进制可以读取未包含这两个字段的旧配置；旧二进制不能解析包含新字段的
|
||||||
|
TextFormat。部署时必须同步更新程序与配置，不能只把新版
|
||||||
|
`grpc_server_task.pb.txt` 复制给旧的 `output/bin/cmvr_es`。
|
||||||
|
|
||||||
|
## 配置验证
|
||||||
|
|
||||||
|
构建后可以用 `protoc --encode` 对单个 TextFormat 文件做语法和字段验证。例如：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
output/bin/protoc \
|
||||||
|
-I protos \
|
||||||
|
--encode=cmvr.config.QuicEdgeRootConfig \
|
||||||
|
protos/cmvr/config/quic_edge_config/quic_edge_config.proto \
|
||||||
|
< cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt \
|
||||||
|
> /tmp/quic_edge_config.pb
|
||||||
|
```
|
||||||
|
|
||||||
|
该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
|
||||||
|
|
||||||
|
## 生产配置
|
||||||
|
|
||||||
|
`cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。
|
||||||
|
|
||||||
|
- 不提交真实设备密码、token、私钥和生产地址；
|
||||||
|
- 证书与私钥放在独立 `certs/`，使用最小读取权限；
|
||||||
|
- 为不同站点维护独立配置根，不在运行时修改仓库样例；
|
||||||
|
- 发布前检查所有 `enable`、IP、端口和设备 ID；
|
||||||
|
- 变更配置 Schema 时同步 Proto 兼容性文档和平台生成代码。
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] TextFormat 可以被对应 root message 解析
|
||||||
|
- [ ] ID、类别和引用路径完全一致
|
||||||
|
- [ ] 新硬件和新网络任务默认关闭
|
||||||
|
- [ ] 参数单位、范围和安全默认值明确
|
||||||
|
- [ ] 没有生产凭据
|
||||||
|
- [ ] 安装覆盖不会丢失现场配置
|
||||||
|
- [ ] 无设备启动仍然成功
|
||||||
25
cmvr-es/config/certs/cmvr-quic-ca.crt
Normal file
25
cmvr-es/config/certs/cmvr-quic-ca.crt
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
-----BEGIN CERTIFICATE-----
|
||||||
|
MIIEKzCCApOgAwIBAgIUTtZCyKM8INYxZKkfwbKdpHKLjn4wDQYJKoZIhvcNAQEL
|
||||||
|
BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQy
|
||||||
|
MFoXDTM2MDcyMTA2NDQyMFowHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENB
|
||||||
|
MIIBojANBgkqhkiG9w0BAQEFAAOCAY8AMIIBigKCAYEAmq2rHldOobaemqNfWggS
|
||||||
|
OVj3inKy6AYjfgtcXUfKs48DDbpZ9gyEd/YPJXA8C2jGPXpxzgnc7a4UCUVQZ8ah
|
||||||
|
ddoJtFcC+Q6BgjeMVqUdUubu5Y9HpkfU3lvnp4KhzvOeFnkKtrCzYIPa2nK3zLc7
|
||||||
|
uCiuLlB+91KQSRXPFbc6N7H/EAfGmUHIwlZGysAkRN7b2TAoR4C7E96JLVtuUQsS
|
||||||
|
VtlEGpunSfuefFzeeZCMS6avLbB+a8Q6yUzLt6pqnheNsDB+jCCXodlJs5XS1AOB
|
||||||
|
W2GOpGFMj7dLoTD+eBIlAlrhWFcwKjzFmtp6LGl/Jy0O+E99X4TL72oNSOdTrO2O
|
||||||
|
iavXABz9IvWR2BrAyo5AKlTJqO6tmZw77iVti8jYi+HsXIVGQKMYwvv5k0jdHeIZ
|
||||||
|
FaDToUbFPP/zj0m8ZraMy+8eNAhScnx6Zs56fcncBuDti6pT+zKisjV1rH/sFvZY
|
||||||
|
wO1UvJlOEbvXrfYLPp58Aqe/toG2nV45a2Q4+x6AETV1AgMBAAGjYzBhMB0GA1Ud
|
||||||
|
DgQWBBRdHz+g2vVmqghDZi2kEHtZfoyHZzAfBgNVHSMEGDAWgBRdHz+g2vVmqghD
|
||||||
|
Zi2kEHtZfoyHZzAPBgNVHRMBAf8EBTADAQH/MA4GA1UdDwEB/wQEAwIBBjANBgkq
|
||||||
|
hkiG9w0BAQsFAAOCAYEASuEoMFiVcg4iTMxO2kshFTJ6LIrqqGXBn+1j+yQ1ennG
|
||||||
|
mqPMo5fBOe/Kp3YWCnREQWu0+EEPEC9qWgIDOIm3v7ch4mZiW31GUOqae6bjprBe
|
||||||
|
er7ySElKGZ5GefKAq++we19A6WHnNxtNAT9BE1VSKUmkxEsnIkuwd+QMmQ9eaIRm
|
||||||
|
8RvzshWdUcyiBJg07sI3rPzPD/YpnfcYlAa2a0+oXJ3o3zlhdbs2S+9fIl+Y/zFN
|
||||||
|
wBaUV6ZNk0RFzOCA+jUq2jU5Y1pODcot3Mp2jCHzp4uTFyVqsoqc5USRB2TUILCP
|
||||||
|
acSbEjD9GnKaNU4miTcftnC85fnBAiZ5btZkXIs72ij+J3VW7QnbcFQG8bElngoR
|
||||||
|
mWizh6ByPQrsvyT0evJjdjpuOzILsVTAn64m0cYpnlR4g5orcEkIJHv4DzMhpXc2
|
||||||
|
CvpcWwk2vJrUuws/LGxDATTAcqtIUIxAkxffqUsdB/6j8fdykNCvXFwNh/uPcawd
|
||||||
|
4kzh6dr+RFzHSMQ6oN16
|
||||||
|
-----END CERTIFICATE-----
|
||||||
40
cmvr-es/config/certs/cmvr-quic-ca.key
Normal file
40
cmvr-es/config/certs/cmvr-quic-ca.key
Normal file
@ -0,0 +1,40 @@
|
|||||||
|
-----BEGIN PRIVATE KEY-----
|
||||||
|
MIIG/wIBADANBgkqhkiG9w0BAQEFAASCBukwggblAgEAAoIBgQCaraseV06htp6a
|
||||||
|
o19aCBI5WPeKcrLoBiN+C1xdR8qzjwMNuln2DIR39g8lcDwLaMY9enHOCdztrhQJ
|
||||||
|
RVBnxqF12gm0VwL5DoGCN4xWpR1S5u7lj0emR9TeW+engqHO854WeQq2sLNgg9ra
|
||||||
|
crfMtzu4KK4uUH73UpBJFc8Vtzo3sf8QB8aZQcjCVkbKwCRE3tvZMChHgLsT3okt
|
||||||
|
W25RCxJW2UQam6dJ+558XN55kIxLpq8tsH5rxDrJTMu3qmqeF42wMH6MIJeh2Umz
|
||||||
|
ldLUA4FbYY6kYUyPt0uhMP54EiUCWuFYVzAqPMWa2nosaX8nLQ74T31fhMvvag1I
|
||||||
|
51Os7Y6Jq9cAHP0i9ZHYGsDKjkAqVMmo7q2ZnDvuJW2LyNiL4exchUZAoxjC+/mT
|
||||||
|
SN0d4hkVoNOhRsU8//OPSbxmtozL7x40CFJyfHpmznp9ydwG4O2LqlP7MqKyNXWs
|
||||||
|
f+wW9ljA7VS8mU4Ru9et9gs+nnwCp7+2gbadXjlrZDj7HoARNXUCAwEAAQKCAYAi
|
||||||
|
e49/pNqWghoTItM9xLldWAhdcMsSH1Y3wgweDoRxqbL2U0I9cFZy0OPZBo+YQowZ
|
||||||
|
RgwLcRbz1MBKPc3aSMWTep95uQEkaVe1YjFS2p3yLqH5AstoFjDuPmJjLWPpuVVX
|
||||||
|
sLXS+wsOO+7lDriLdpjlagpEsHTRqbIZXPeM4YtkwbV5SyZ64ZfCPU4sYo/jW6R6
|
||||||
|
43nDUP9Dw3Nk7XJnNlbpDigY33T4sRPIqUJ+qtsf/WGlx6gzWax6Vni+8gqxQlIe
|
||||||
|
80XG6dHCM3UGFEdGF64PB2FuprsPWBSSau7L7I/gagiIuQbhs9VYXOanz1MQizMU
|
||||||
|
ZdK8cqmG/bpP+uY4VQLJ5Lxu87boEllGFHBOYTCjWq0zatzGMt1/DeggLnjgfC90
|
||||||
|
5kOzlxl403j+4K8nqiHoKpAbOatwTxFz1up6XSecsarE1RKsCq8sIH+FjjCfO1Db
|
||||||
|
hBfv2a7/XemdcxHtl9Q/EOSDaAFOUQfFMDAc3rCH7/vonTzaaZq5ucWmjlBsv2EC
|
||||||
|
gcEAz+cH+3x09pXVOnM7zlWrGAQk69Fj/0BGs7g3GFf8ZvI/kkbez+9J1QGItxj+
|
||||||
|
7Dm4Ay0kgxYi3DxFWbPLHTbFpVg2CyL08IQZSXJzokwz2V/HTd1a+OgzgB/dBNDc
|
||||||
|
V+XnxJI89+SI3VhOQZJG625l//lhHWCy2u0ULE7ZlK3ao95L86bSXQHI4dZ2nWXl
|
||||||
|
0f4ISJmm4qkEJAY+DkQeF3UZLumd1NRxy0LBdbYVM/wZIN+sGsRdQqQ9f+ik6+aX
|
||||||
|
YrjhAoHBAL52dJzIPIN1Z/Y9qxUrVLPE7K8Mm8KLaFIl/ztl3TFqaUvErHi66nx3
|
||||||
|
MTwEnrQIBI31OKxB6Or2JLxYMvDTSVRV1K2tY53+K6Q8giWU4x3zFc/svyl35gkd
|
||||||
|
U1bLdfxXqKMLrvlmVWj6/RMoH7vd/aTcidYVvwOZgtL+6RahS4LfZ9qXAyREFAJa
|
||||||
|
VSI0HW5LLiOBLTegztT1CBVe7xb78bWFMiUkxre4XsLZYwFPzXzVPh6HloDcACDL
|
||||||
|
FPwLrfJrFQKBwQCvGDl90UzEnF4v0vsshLQLDvp1bS1VvTGOjPhB1WBq510o+e0P
|
||||||
|
nM1Gyvr0keWo19elPTDCAjOr3kreCHFpEkcVQRyK9o7pvad6Vx0SNDF6wpKdfm7u
|
||||||
|
sMknAC7prmnU0XkH8c3NTTkDiiqmSObXw2u+UK48ysL3ZLIXuvS+pkk8t6yp8Pa8
|
||||||
|
hBNGOJQ/baFH4TXixx1pScWF/YfoBfB9+w4Rl4loxN9tu7QpSgfDd29GY3qUNIsC
|
||||||
|
5EYzYqD7WIJpD6ECgcEAkdIfdenYas14yw5r7ck/EGO00lDU8B3LwRlWUCOtNihC
|
||||||
|
dcAeTFDPNnwLNehTmYKJ+iXFPh04Nqw9c/YTCk651dfg/RfDLTNsNlIdUqirOkLi
|
||||||
|
cE7SDO2/MTtCkzEzI//5HNvVGx0+RyHioMgXg75yc8ZlwYLku9zMTL7dtnXHWmux
|
||||||
|
F6qGvT1iFGsUwxsjbU4iBQzhkbWMpX70sWf9pZs/c7qGqel+OyrtYkENi/ONYAXj
|
||||||
|
iXxFvmKxtmnFpzNJ+lABAoHBAJjs8jfEXJnJP80t93Bxg6NmfKhxUEm4ovcIeR8U
|
||||||
|
lEfb8Ind1ICrBWGVGOAkirTFZgFAAlEjne+lT1FYgf19oC+XbxFdl/e02M8KfflS
|
||||||
|
01+0/t6lEpefiU0dfW0dWtXOPaAVvpNgMkeMgBzB2Yz7ZNUrMypfCGk289qveWxB
|
||||||
|
8UbO+QLjTI+cFvPsmR95Eti/nAjd2fXGeqbR+z+DdQnz4BvXLSCQCw4on2+T+mpw
|
||||||
|
BprrEQmcdeZA8LE1zgcKtkZR1w==
|
||||||
|
-----END PRIVATE KEY-----
|
||||||
1
cmvr-es/config/certs/cmvr-quic-ca.srl
Normal file
1
cmvr-es/config/certs/cmvr-quic-ca.srl
Normal file
@ -0,0 +1 @@
|
|||||||
|
2C945D70B02014891B6E09D57E377CEFB6D18498
|
||||||
22
cmvr-es/config/certs/quic-gateway.crt
Normal file
22
cmvr-es/config/certs/quic-gateway.crt
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
-----BEGIN CERTIFICATE-----
|
||||||
|
MIIDuzCCAiOgAwIBAgIULJRdcLAgFIkbbgnVfjd877bRhJgwDQYJKoZIhvcNAQEL
|
||||||
|
BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQz
|
||||||
|
NloXDTI4MTAyNjA2NDQzNlowGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIw
|
||||||
|
DQYJKoZIhvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuK
|
||||||
|
pzilz5uqCKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuO
|
||||||
|
bhSlG1EIhkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVw
|
||||||
|
kitCYlyTza5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG8
|
||||||
|
0/77AeiL0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg068
|
||||||
|
4705vbFInBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEA
|
||||||
|
AaN4MHYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAwEwYDVR0lBAww
|
||||||
|
CgYIKwYBBQUHAwEwHQYDVR0OBBYEFLmoEtsglxm3mXh4l8OF31bt2ZhnMB8GA1Ud
|
||||||
|
IwQYMBaAFF0fP6Da9WaqCENmLaQQe1l+jIdnMA0GCSqGSIb3DQEBCwUAA4IBgQAR
|
||||||
|
eK1mD9rJkzHe4OusimQfcuDQW+0J32e4T/34RHlW+lIj7botFaElXIzO9S80tDwq
|
||||||
|
4d4ozNPKysqgJN9hv/BBMzJpZLwP2XozPaGLTNl1jRTCc9UhFUPrUeu0LbpQGBfC
|
||||||
|
6Ghq42V94zPAw4lnMujnkq8botk21hclbJORQ9kblXP31IdWCgiKSFLy1NTBmQmc
|
||||||
|
IxmR+SldMWYrWGWv/0I85AeMu6HR3+NKHmzDblm1HUHFekyC1f7sypNG+D1r8ab2
|
||||||
|
GSijoCMKHSEOm81Vl/j5bgWQygnnIOhsLOUf2DZO6jC+VZmTKpMEJNHgwk23WBlu
|
||||||
|
FYz/X9p5z9ZucL8aBxegj7G1fI5Ik2O05+LLeqJMfspAm6ZcnRDxCVZWQ+K/dAle
|
||||||
|
fz+gTASzhHTsjEBeiX46LP0L2PVyBiPvNtD66e52LoM5ZigechZq91niLO+ipWdh
|
||||||
|
xq7ryJzmaAEQKPuYaoswriWzJam07ywX9yupGHzBA6vVgGy0jPAsnGCuZXTfDyA=
|
||||||
|
-----END CERTIFICATE-----
|
||||||
17
cmvr-es/config/certs/quic-gateway.csr
Normal file
17
cmvr-es/config/certs/quic-gateway.csr
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
-----BEGIN CERTIFICATE REQUEST-----
|
||||||
|
MIICpDCCAYwCAQAwGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIwDQYJKoZI
|
||||||
|
hvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuKpzilz5uq
|
||||||
|
CKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuObhSlG1EI
|
||||||
|
hkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVwkitCYlyT
|
||||||
|
za5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG80/77AeiL
|
||||||
|
0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg0684705vbFI
|
||||||
|
nBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEAAaBHMEUG
|
||||||
|
CSqGSIb3DQEJDjE4MDYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAw
|
||||||
|
EwYDVR0lBAwwCgYIKwYBBQUHAwEwDQYJKoZIhvcNAQELBQADggEBAGIcLeCE344z
|
||||||
|
PENI1/oONVHBzMMt5VN0P8jbkJOFgZ3a6AUhfqAmDNBr+8SBym+cX2Y9Q2BsaWAu
|
||||||
|
TKOBN+fs+fh5/NqF0hTNvmXzp89NFK5SlsTjoC21HJvK1HTNuNW8drOxNfWgFW3/
|
||||||
|
gSCutcsWS9hVtYrV2FHQzOvVXvXKfmuTkZE8g92P3BCkvRm+ORxI0QfVS81/Ibnv
|
||||||
|
Yl+t/o2QCphdP+1OWzU6+Pccqi2xCyIc6jqCmh0b01zg265PTTOfICO6WQNtYcOn
|
||||||
|
308avDDpTneGl5vWRVdmmiXPqkc+dCesoguIEDUlerFpDKKY81P2GTpfjki93K7Z
|
||||||
|
2DJPs4P1cU0=
|
||||||
|
-----END CERTIFICATE REQUEST-----
|
||||||
@ -1,9 +0,0 @@
|
|||||||
agv {
|
|
||||||
agvs {
|
|
||||||
id: "agv_1"
|
|
||||||
my_agv {
|
|
||||||
ip: "127.0.0.1"
|
|
||||||
port: 8080
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
45
cmvr-es/config/devices/agv/src1100.pb.txt
Normal file
45
cmvr-es/config/devices/agv/src1100.pb.txt
Normal file
@ -0,0 +1,45 @@
|
|||||||
|
agv {
|
||||||
|
agvs {
|
||||||
|
id: "agv_1"
|
||||||
|
my_agv {
|
||||||
|
ip: "127.0.0.1"
|
||||||
|
port: 8080
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
agvs {
|
||||||
|
id: "src1100"
|
||||||
|
src1100_agv {
|
||||||
|
ip: "192.168.192.5"
|
||||||
|
port_status: 19204
|
||||||
|
port_control: 19205
|
||||||
|
port_nav: 19206
|
||||||
|
port_config: 19207
|
||||||
|
port_other: 19210
|
||||||
|
port_push: 19301
|
||||||
|
recv_timeout_ms: 1000
|
||||||
|
enable_state_push: true
|
||||||
|
state_push_interval_ms: 200
|
||||||
|
state_push_included_fields: "x"
|
||||||
|
state_push_included_fields: "y"
|
||||||
|
state_push_included_fields: "angle"
|
||||||
|
state_push_included_fields: "vx"
|
||||||
|
state_push_included_fields: "vy"
|
||||||
|
state_push_included_fields: "w"
|
||||||
|
state_push_included_fields: "battery_level"
|
||||||
|
state_push_included_fields: "battery_temp"
|
||||||
|
state_push_included_fields: "charging"
|
||||||
|
state_push_included_fields: "voltage"
|
||||||
|
state_push_included_fields: "current"
|
||||||
|
state_push_included_fields: "current_map"
|
||||||
|
state_push_included_fields: "current_station"
|
||||||
|
state_push_included_fields: "confidence"
|
||||||
|
state_push_included_fields: "emergency"
|
||||||
|
state_push_included_fields: "fatals"
|
||||||
|
state_push_included_fields: "errors"
|
||||||
|
enable_map_update: true
|
||||||
|
map_update_interval_ms: 1000
|
||||||
|
map_update_history_size: 8
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -4,7 +4,7 @@ arm {
|
|||||||
|
|
||||||
vendor {
|
vendor {
|
||||||
brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM
|
brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM
|
||||||
ip: "192.168.1.100"
|
ip: "192.168.192.18"
|
||||||
port: 30004
|
port: 30004
|
||||||
dof: 6
|
dof: 6
|
||||||
joint_names: "joint_1"
|
joint_names: "joint_1"
|
||||||
|
|||||||
@ -146,4 +146,43 @@ camera {
|
|||||||
sync: false
|
sync: false
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
cameras {
|
||||||
|
id: "hikvision_cam"
|
||||||
|
hikvision {
|
||||||
|
ip: "192.168.192.64"
|
||||||
|
port: 8000
|
||||||
|
username: "admin"
|
||||||
|
password: "okwy1688"
|
||||||
|
channel: 1
|
||||||
|
stream_type: 0
|
||||||
|
link_mode: 0
|
||||||
|
width: 1920
|
||||||
|
height: 1080
|
||||||
|
fps: 25
|
||||||
|
codec: "H264"
|
||||||
|
camera_mode: CAMERA_MODE_VIDEO
|
||||||
|
stream_mode: STREAM_MODE_RGB
|
||||||
|
buffer_size: 30
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cameras {
|
||||||
|
id: "hikvision_thermal_cam"
|
||||||
|
hikvision {
|
||||||
|
ip: "192.168.192.65"
|
||||||
|
port: 8000
|
||||||
|
username: "admin"
|
||||||
|
password: "okwy1688"
|
||||||
|
channel: 1
|
||||||
|
stream_type: 0
|
||||||
|
link_mode: 0
|
||||||
|
width: 384
|
||||||
|
height: 288
|
||||||
|
fps: 50
|
||||||
|
codec: "H264"
|
||||||
|
camera_mode: CAMERA_MODE_VIDEO
|
||||||
|
stream_mode: STREAM_MODE_RGB
|
||||||
|
buffer_size: 30
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -3,7 +3,7 @@ microphone {
|
|||||||
id: "mic1"
|
id: "mic1"
|
||||||
ffmpeg {
|
ffmpeg {
|
||||||
channels: 2
|
channels: 2
|
||||||
sampleRate: 44100
|
sampleRate: 48000
|
||||||
volume: 100
|
volume: 100
|
||||||
input_device: "default"
|
input_device: "default"
|
||||||
}
|
}
|
||||||
|
|||||||
@ -100,7 +100,7 @@ device_manager {
|
|||||||
id: "huayan_arm"
|
id: "huayan_arm"
|
||||||
type: DEVICE_TYPE_ROBOT_ARM
|
type: DEVICE_TYPE_ROBOT_ARM
|
||||||
config_file: "devices/arm/huayan_arm.pb.txt"
|
config_file: "devices/arm/huayan_arm.pb.txt"
|
||||||
enable: true
|
enable: false
|
||||||
}
|
}
|
||||||
|
|
||||||
devices {
|
devices {
|
||||||
@ -109,11 +109,42 @@ device_manager {
|
|||||||
config_file: "devices/biohead/bio_head.pb.txt"
|
config_file: "devices/biohead/bio_head.pb.txt"
|
||||||
enable: false
|
enable: false
|
||||||
}
|
}
|
||||||
|
|
||||||
devices {
|
devices {
|
||||||
id: "agv_1"
|
id: "src1100"
|
||||||
type: DEVICE_TYPE_AGV
|
type: DEVICE_TYPE_AGV
|
||||||
config_file: "devices/agv/agv.pb.txt"
|
config_file: "devices/agv/src1100.pb.txt"
|
||||||
enable: false
|
enable: false
|
||||||
}
|
}
|
||||||
|
|
||||||
|
devices {
|
||||||
|
id: "hikvision_cam"
|
||||||
|
type: DEVICE_TYPE_CAMERA
|
||||||
|
config_file: "devices/camera/camera.pb.txt"
|
||||||
|
# Host-development default: keep physical cameras disabled.
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
|
devices {
|
||||||
|
id: "hikvision_thermal_cam"
|
||||||
|
type: DEVICE_TYPE_CAMERA
|
||||||
|
config_file: "devices/camera/camera.pb.txt"
|
||||||
|
# Host-development default: keep physical cameras disabled.
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
|
|
||||||
|
devices {
|
||||||
|
id: "mic1"
|
||||||
|
type: DEVICE_TYPE_MICROPHONE
|
||||||
|
config_file: "devices/microphone/microphone.pb.txt"
|
||||||
|
# Host-development default: keep physical audio devices disabled.
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
|
|
||||||
|
devices {
|
||||||
|
id: "spk1"
|
||||||
|
type: DEVICE_TYPE_SPEAKER
|
||||||
|
config_file: "devices/speaker/speaker.pb.txt"
|
||||||
|
# Host-development default: keep physical audio devices disabled.
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -20,6 +20,14 @@ task_manager {
|
|||||||
run_mode: TASK_RUN_MODE_PERIODIC_STEP
|
run_mode: TASK_RUN_MODE_PERIODIC_STEP
|
||||||
control_period_s: 0.002
|
control_period_s: 0.002
|
||||||
config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
|
config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
|
tasks {
|
||||||
|
id: "quic_edge"
|
||||||
|
type: TASK_TYPE_QUIC_EDGE
|
||||||
|
run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
|
||||||
|
config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt"
|
||||||
|
# Host-development default: no QUIC Gateway or physical media devices.
|
||||||
enable: true
|
enable: true
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -3,4 +3,6 @@ grpc_server {
|
|||||||
host: "0.0.0.0"
|
host: "0.0.0.0"
|
||||||
port: "50052"
|
port: "50052"
|
||||||
enable_reflection: true
|
enable_reflection: true
|
||||||
|
camera_stream_max_pending_frames: 2
|
||||||
|
camera_stream_max_frame_age_ms: 250
|
||||||
}
|
}
|
||||||
|
|||||||
65
cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt
Normal file
65
cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt
Normal file
@ -0,0 +1,65 @@
|
|||||||
|
quic_edge {
|
||||||
|
id: "quic_edge"
|
||||||
|
|
||||||
|
# Task enablement is controlled by manager/task_manager.pb.txt. Configure a
|
||||||
|
# reachable QUIC Gateway and TLS policy before enabling the task there.
|
||||||
|
server_host: "quic-gateway.example.com"
|
||||||
|
server_port: 4433
|
||||||
|
alpn: "cmvr-quic-edge/1"
|
||||||
|
node_id: "cmvr-edge"
|
||||||
|
software_version: "0.1"
|
||||||
|
|
||||||
|
# The existing cmvr-es gRPC server remains the robot-control endpoint. "auto"
|
||||||
|
# selects a usable address from the interface snapshot sent at registration
|
||||||
|
# and on every heartbeat.
|
||||||
|
grpc_endpoint_host: "auto"
|
||||||
|
grpc_endpoint_port: 50052
|
||||||
|
grpc_endpoint_tls: false
|
||||||
|
include_loopback_interfaces: false
|
||||||
|
|
||||||
|
# Local heartbeat period. The Gateway keeps this value when its registration
|
||||||
|
# response returns heartbeat_interval_ms=0; a non-zero response overrides it.
|
||||||
|
heartbeat_interval_ms: 5000
|
||||||
|
control_response_timeout_ms: 1000
|
||||||
|
|
||||||
|
tls {
|
||||||
|
ca_file: "certs/quic_gateway_ca.pem"
|
||||||
|
certificate_file: "certs/cmvr_edge_cert.pem"
|
||||||
|
private_key_file: "certs/cmvr_edge_key.pem"
|
||||||
|
server_name: "quic-gateway.example.com"
|
||||||
|
allow_insecure: false
|
||||||
|
}
|
||||||
|
|
||||||
|
reconnect {
|
||||||
|
initial_delay_ms: 500
|
||||||
|
maximum_delay_ms: 30000
|
||||||
|
multiplier: 2.0
|
||||||
|
jitter_percent: 20
|
||||||
|
connect_timeout_ms: 5000
|
||||||
|
}
|
||||||
|
|
||||||
|
maximum_datagram_bytes: 1200
|
||||||
|
maximum_control_frame_bytes: 1048576
|
||||||
|
# With 1200-byte DATAGRAMs and a 512-entry queue, 524288 stays below
|
||||||
|
# the atomic batch capacity while reserving slots for control messages.
|
||||||
|
maximum_frame_bytes: 524288
|
||||||
|
datagram_send_queue_depth: 512
|
||||||
|
media_poll_interval_ms: 2
|
||||||
|
|
||||||
|
# Zero media tracks is valid and keeps registration, IP reporting and
|
||||||
|
# heartbeat active. Add tracks only for devices enabled in DeviceManager.
|
||||||
|
# tracks {
|
||||||
|
# track_id: 1
|
||||||
|
# source_kind: SOURCE_KIND_CAMERA
|
||||||
|
# device_id: "right_hand_cam"
|
||||||
|
# source_track_id: "right_hand_cam/video/color"
|
||||||
|
# enable: true
|
||||||
|
# }
|
||||||
|
# tracks {
|
||||||
|
# track_id: 2
|
||||||
|
# source_kind: SOURCE_KIND_MICROPHONE
|
||||||
|
# device_id: "mic1"
|
||||||
|
# source_track_id: "mic1/audio/main"
|
||||||
|
# enable: true
|
||||||
|
# }
|
||||||
|
}
|
||||||
369
cmvr-es/devices/README.md
Normal file
369
cmvr-es/devices/README.md
Normal file
@ -0,0 +1,369 @@
|
|||||||
|
# Devices 模块开发指南
|
||||||
|
|
||||||
|
`devices/` 屏蔽厂商 SDK、通信总线和硬件型号差异，对 manager、service、task 和算法层提供稳定的设备能力接口。
|
||||||
|
|
||||||
|
仓库真实目录名是 `cmvr-es/devices/`。设备层使用复数 `devices`，不是 `cmvr_es/device`。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 创建链路
|
||||||
|
|
||||||
|
```text
|
||||||
|
config/cmvr_es.pb.txt
|
||||||
|
-> config/manager/device_manager.pb.txt
|
||||||
|
-> DeviceManager
|
||||||
|
-> DeviceFactory
|
||||||
|
按 DeviceConfigEntry::DeviceType 选择设备大类
|
||||||
|
-> CameraFactory / AGVFactory / RobotArmFactory / ...
|
||||||
|
按设备配置 oneof backend 选择厂商实现
|
||||||
|
-> 具体设备类
|
||||||
|
-> AbstractDevice 和设备大类抽象接口
|
||||||
|
```
|
||||||
|
|
||||||
|
关键文件：
|
||||||
|
|
||||||
|
- [`abstract_device.h`](abstract_device.h)
|
||||||
|
- [`device_types.h`](device_types.h)
|
||||||
|
- [`../manager/device_manager/include/device_manager.h`](../manager/device_manager/include/device_manager.h)
|
||||||
|
- [`../manager/device_manager/src/device_factory.cpp`](../manager/device_manager/src/device_factory.cpp)
|
||||||
|
- [`../../protos/cmvr/config/device_manager_config/device_manager_config.proto`](../../protos/cmvr/config/device_manager_config/device_manager_config.proto)
|
||||||
|
- [`../config/manager/device_manager.pb.txt`](../config/manager/device_manager.pb.txt)
|
||||||
|
|
||||||
|
协议层和业务任务不应直接依赖厂商 SDK 类型。厂商错误码、报文和连接细节由具体后端转换为抽象接口的通用语义。
|
||||||
|
|
||||||
|
## 当前可由配置创建的设备
|
||||||
|
|
||||||
|
| 大类 | 抽象接口 | 类别工厂 | 当前可选后端 |
|
||||||
|
| --- | --- | --- | --- |
|
||||||
|
| Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision |
|
||||||
|
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 |
|
||||||
|
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan |
|
||||||
|
| DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 |
|
||||||
|
| Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
|
||||||
|
| Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
|
||||||
|
| BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
|
||||||
|
| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group |
|
||||||
|
|
||||||
|
代码目录存在不等于已经接入配置创建链：
|
||||||
|
|
||||||
|
- MechMind Proto 和实现仍存在，但当前 CameraFactory 明确拒绝创建；
|
||||||
|
- MujocoCamera 有实现并参与部分构建，但当前没有 CameraFactory 分支；
|
||||||
|
- Battery、Gripper、Robot、CanBus 等抽象或实现不一定已注册到 DeviceFactory；
|
||||||
|
- 所有已注册设备共用一个全局 ID 命名空间。
|
||||||
|
|
||||||
|
新增能力前先确认“已有代码”“可被 CMake 构建”“可被 Factory 创建”“可被 DeviceManager 配置启用”四个状态，不要混为一谈。
|
||||||
|
|
||||||
|
## 新增同类厂商后端
|
||||||
|
|
||||||
|
以新增 Camera 后端为例。
|
||||||
|
|
||||||
|
### 1. 扩展配置 Proto
|
||||||
|
|
||||||
|
修改：
|
||||||
|
|
||||||
|
```text
|
||||||
|
protos/cmvr/config/camera_config/camera_config.proto
|
||||||
|
```
|
||||||
|
|
||||||
|
新增厂商 config，并加入 `CameraDeviceConfig.oneof backend`。只使用新的字段 tag，不复用 reserved 或已发布 tag。
|
||||||
|
|
||||||
|
### 2. 新建后端目录
|
||||||
|
|
||||||
|
```text
|
||||||
|
camera/vendor_camera/
|
||||||
|
├── CMakeLists.txt
|
||||||
|
├── include/
|
||||||
|
│ └── vendor_camera.h
|
||||||
|
├── src/
|
||||||
|
│ └── vendor_camera.cpp
|
||||||
|
└── tests/
|
||||||
|
└── vendor_camera_test.cpp
|
||||||
|
```
|
||||||
|
|
||||||
|
### 3. 实现抽象接口
|
||||||
|
|
||||||
|
至少实现：
|
||||||
|
|
||||||
|
- `typeName()`；
|
||||||
|
- `init()`；
|
||||||
|
- 实际支持的 `start()` / `stop()`；
|
||||||
|
- 状态查询和该类别核心能力；
|
||||||
|
- 若能提供运行时健康信息，实现无阻塞的 `healthSnapshot()`；
|
||||||
|
- 若支持实时媒体，完整实现流接口和并发停止。
|
||||||
|
|
||||||
|
不要为了厂商特例向抽象类加入 SDK handle、私有报文或厂商专有结构。只有多个后端都需要的稳定语义才进入抽象接口或 `common/types/`。
|
||||||
|
|
||||||
|
### 4. 类别工厂注册
|
||||||
|
|
||||||
|
在 [`camera/camera_factory.h`](camera/camera_factory.h) 的 `backend_case()` 增加创建分支。
|
||||||
|
|
||||||
|
AGV、DexHand、Microphone、Speaker 等遵循相同模式。新增同类后端通常不需要修改全局 DeviceFactory。
|
||||||
|
|
||||||
|
### 5. CMake 聚合
|
||||||
|
|
||||||
|
1. 在 `camera/CMakeLists.txt` 增加 `add_subdirectory(vendor_camera)`；
|
||||||
|
2. 让类别 target 链接新后端 target；
|
||||||
|
3. 安装需要随应用分发的共享库；
|
||||||
|
4. 厂商 SDK 路径使用 `dependency/${ARCH}/third_party/...`，不能硬编码 x86；
|
||||||
|
5. 需要特殊 RPATH 时参考 Hikvision、Huayan 等现有实现。
|
||||||
|
|
||||||
|
推荐 target 形式：
|
||||||
|
|
||||||
|
```cmake
|
||||||
|
add_library(vendor_camera SHARED
|
||||||
|
src/vendor_camera.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(vendor_camera
|
||||||
|
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(vendor_camera
|
||||||
|
PUBLIC cmvr_es::proto
|
||||||
|
PRIVATE vendor_sdk
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(cmvr_es::device::vendor_camera ALIAS vendor_camera)
|
||||||
|
install(TARGETS vendor_camera LIBRARY DESTINATION lib)
|
||||||
|
```
|
||||||
|
|
||||||
|
### 6. 默认配置
|
||||||
|
|
||||||
|
在类别集合配置增加实例，并在 DeviceManager 配置增加同 ID 条目：
|
||||||
|
|
||||||
|
```protobuf
|
||||||
|
devices {
|
||||||
|
id: "camera_front"
|
||||||
|
type: DEVICE_TYPE_CAMERA
|
||||||
|
config_file: "devices/camera/camera.pb.txt"
|
||||||
|
enable: false
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
新硬件默认 `enable: false`，确保无设备开发主机仍可启动。
|
||||||
|
|
||||||
|
## 新增全新设备大类
|
||||||
|
|
||||||
|
当现有抽象无法表达新设备能力时，还需要：
|
||||||
|
|
||||||
|
1. 在 [`device_types.h`](device_types.h) 增加 `DeviceKind` 和 `toString()`；
|
||||||
|
2. 新增类别抽象接口；
|
||||||
|
3. 将稳定公共数据放入 `../common/types/<category>/`；
|
||||||
|
4. 新增配置 Proto；
|
||||||
|
5. 在 `DeviceConfigEntry::DeviceType` 使用新的 enum 数值；
|
||||||
|
6. 在 `DeviceFactory::DeviceFactory()` 注册 creator；
|
||||||
|
7. 更新 DeviceManager 的 type-to-string 日志映射；
|
||||||
|
8. 为 `getDevice<AbstractNewDevice>()` 增加显式模板实例化；
|
||||||
|
9. 更新 `devices/CMakeLists.txt`、类别 CMake 和 device_manager target 链接；
|
||||||
|
10. 若平台需要访问，独立增加 API Proto 和 gRPC service。
|
||||||
|
|
||||||
|
不得复用 `device_manager_config.proto` 中已 reserved 的 enum 数值或名称。新增设备类别不会自动生成平台 RPC。
|
||||||
|
|
||||||
|
## 配置和 ID
|
||||||
|
|
||||||
|
集合类型设备必须满足：
|
||||||
|
|
||||||
|
```text
|
||||||
|
DeviceConfigEntry.id
|
||||||
|
==
|
||||||
|
CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id
|
||||||
|
```
|
||||||
|
|
||||||
|
厂商 backend 内部 ID 可以为空，由类别工厂补齐；若填写，也必须与外层 ID 相同。
|
||||||
|
|
||||||
|
其他约束：
|
||||||
|
|
||||||
|
- 相对配置路径以根 `cmvr_es.pb.txt` 所在目录解析；
|
||||||
|
- 生产密码、token 和证书不得提交到样例配置；
|
||||||
|
- 单个设备 init 失败时不会进入可用对象表，进程仍可能继续启动；失败条目会保留
|
||||||
|
在 DeviceManager 状态快照中，其中已启用的失败设备会通过 QUIC heartbeat
|
||||||
|
上报，禁用设备不会上报；
|
||||||
|
- 有初始化依赖的设备按配置顺序排列，例如 MotorSystem 在依赖它的 RobotArm 前；
|
||||||
|
- DeviceManager stop 遍历 unordered_map，不能依赖跨设备停止顺序；
|
||||||
|
- DeviceManager 支持并发查询、状态快照和动态注册，但动态设备不会自动补执行
|
||||||
|
已经发生的 `start()`，当前也没有设备移除或完整热插拔生命周期。
|
||||||
|
|
||||||
|
配置细节见 [`../config/README.md`](../config/README.md)。
|
||||||
|
|
||||||
|
## 生命周期约定
|
||||||
|
|
||||||
|
| 接口 | 当前语义 |
|
||||||
|
| --- | --- |
|
||||||
|
| 构造函数 | 保存配置和轻量校验，不启动长期工作线程 |
|
||||||
|
| `init()` | DeviceManager 构造期间对启用设备调用 |
|
||||||
|
| `start()` | 当前 main 不统一调用，由 RPC、媒体或其他 owner 显式触发 |
|
||||||
|
| `startStreaming()` | 获取一个实时流生产租约 |
|
||||||
|
| `stopStreaming()` | 释放租约，最后一份租约停止生产者 |
|
||||||
|
| `stop()` | 完整停止设备，必须幂等 |
|
||||||
|
| `update()` | 当前没有统一 scheduler 自动调用 |
|
||||||
|
| 析构函数 | 回收线程、SDK callback、socket、fd 和 handle |
|
||||||
|
|
||||||
|
实现要求：
|
||||||
|
|
||||||
|
- init/start/stop 的重复调用有明确结果；
|
||||||
|
- stop 和最后一次 stopStreaming 返回前停止所有发布；
|
||||||
|
- 不持有 worker 退出所需的锁执行 join；
|
||||||
|
- SDK callback 不获取停止路径长期持有的控制锁；
|
||||||
|
- getState 使用与状态写入相同的锁；
|
||||||
|
- 含 `std::string`、vector 等状态不能无锁复制；
|
||||||
|
- `healthSnapshot()` 只能读取已经缓存的内存状态，必须线程安全，不能同步访问
|
||||||
|
SDK、网络、串口或设备总线；
|
||||||
|
- 无法提供可信健康状态时返回 `UNKNOWN`，不能用“没有观察到错误”冒充健康；
|
||||||
|
- 析构函数调用安全停止路径；
|
||||||
|
- callback 捕获对象前保证 owner 生命周期。
|
||||||
|
|
||||||
|
并发停止可参考：
|
||||||
|
|
||||||
|
- [`camera/uvc_camera/`](camera/uvc_camera/)
|
||||||
|
- [`camera/hikvision_camera/`](camera/hikvision_camera/)
|
||||||
|
- [`camera/hikvision_camera/tests/`](camera/hikvision_camera/tests/)
|
||||||
|
|
||||||
|
当前 main 收到退出信号只停止 TaskManager，没有显式调用 DeviceManager stop，设备析构仍必须可靠。
|
||||||
|
|
||||||
|
## 摄像头与麦克风实时流
|
||||||
|
|
||||||
|
设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
|
||||||
|
|
||||||
|
当前 Hub 轨道：
|
||||||
|
|
||||||
|
```text
|
||||||
|
<camera-id>/video/color
|
||||||
|
<microphone-id>/audio/main
|
||||||
|
```
|
||||||
|
|
||||||
|
当前 adapter 只把 `StreamFrameData.rgbFrame` 注册为彩色视频轨道；depthFrame 尚未注册为 Hub 深度轨道。
|
||||||
|
|
||||||
|
### Camera 完整编码帧
|
||||||
|
|
||||||
|
每个 access unit 应正确填写：
|
||||||
|
|
||||||
|
- `rgbFrame`
|
||||||
|
- `codec`：H.264 或 H.265
|
||||||
|
- `width`、`height`、`fps`
|
||||||
|
- `bKey`
|
||||||
|
- `stream_epoch`
|
||||||
|
- `sequence`
|
||||||
|
- `capture_monotonic_ns`
|
||||||
|
- `capture_utc_ns`
|
||||||
|
- `source_timestamp`：设备 SDK 提供的原始时间戳；单位和时钟域由设备定义，未知
|
||||||
|
时保持 0，不能直接当成 Unix 时间；
|
||||||
|
- `source_frame_number`：设备 SDK 提供的原始帧号，未知时保持 0；
|
||||||
|
- `pts`、`dts`
|
||||||
|
- `time_base_num`、`time_base_den`
|
||||||
|
- `duration`
|
||||||
|
- `discontinuity`
|
||||||
|
- `codec_config_generation`
|
||||||
|
- `codec_config`
|
||||||
|
|
||||||
|
H.264/H.265 后端必须识别关键帧，并尽量实现 `requestKeyFrame()`。
|
||||||
|
|
||||||
|
Hikvision 后端优先采用 SDK 回调中的有效帧率，并保留 SDK 的 64 位原始时间戳和帧号；
|
||||||
|
SDK 帧率无效时才回退到配置的 `fps`。这些字段用于跨层诊断，设备层不应在不了解
|
||||||
|
SDK 时钟语义时擅自换算。
|
||||||
|
|
||||||
|
### Microphone 完整音频包
|
||||||
|
|
||||||
|
应正确填写：
|
||||||
|
|
||||||
|
- `data`
|
||||||
|
- `sample_rate`
|
||||||
|
- `channels`
|
||||||
|
- `format` 和 `codec`
|
||||||
|
- `nb_samples`
|
||||||
|
- sequence、时间戳、time base、duration
|
||||||
|
- stream epoch、discontinuity 和 codec generation
|
||||||
|
|
||||||
|
### 生产者重启
|
||||||
|
|
||||||
|
采集/编码生产者真正停止并重新启动时：
|
||||||
|
|
||||||
|
1. 增加 `stream_epoch`；
|
||||||
|
2. 将 source sequence 重置为 0；
|
||||||
|
3. 增加 `codec_config_generation`；
|
||||||
|
4. 确保后续帧携带完整的新编码元数据；
|
||||||
|
5. 将第一帧标记为 discontinuity；
|
||||||
|
6. 视频从关键帧恢复输出。
|
||||||
|
|
||||||
|
设备后端不直接发布 `TrackDescriptor`。`device_media_source_adapter.cpp` 会根据帧元数据生成或更新 descriptor。
|
||||||
|
|
||||||
|
### 运行期编码配置变化
|
||||||
|
|
||||||
|
编码器没有重启、只在同一 stream epoch 内改变分辨率、codec config 等参数时：
|
||||||
|
|
||||||
|
1. 保持 `stream_epoch` 不变；
|
||||||
|
2. 保持 sequence 连续递增；
|
||||||
|
3. 增加 `codec_config_generation`，或让其他描述字段反映变化；
|
||||||
|
4. 在后续帧中携带新元数据；
|
||||||
|
5. 标记 discontinuity；
|
||||||
|
6. H.264/H.265 从新的关键帧恢复。
|
||||||
|
|
||||||
|
adapter 检测到描述变化后创建新 descriptor，设备后端不要自行维护协议侧 descriptor 状态。
|
||||||
|
|
||||||
|
## 设备侧环形队列安全
|
||||||
|
|
||||||
|
现有设备后端多使用 `SPMCRingBuffer<T>`：
|
||||||
|
|
||||||
|
- 只允许一个逻辑生产者；
|
||||||
|
- 每个消费者独立持有读游标；
|
||||||
|
- 同一读游标不能跨线程并发访问；
|
||||||
|
- 不对同一路读取混用无参 `pop()` 和带游标 `pop(index)`；
|
||||||
|
- 需要最新帧时使用 `getLatest(index)`；
|
||||||
|
- 不要先取 head 再分两步读取，避免检查/读取竞态；
|
||||||
|
- 满队列覆盖旧数据是实时媒体的预期行为；
|
||||||
|
- `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。
|
||||||
|
|
||||||
|
MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
|
||||||
|
|
||||||
|
发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。
|
||||||
|
|
||||||
|
## 测试要求
|
||||||
|
|
||||||
|
至少覆盖：
|
||||||
|
|
||||||
|
- 配置缺失、非法参数和 ID 不一致；
|
||||||
|
- Factory 选择正确后端；
|
||||||
|
- init/start/stop 重复执行；
|
||||||
|
- init 失败后无残留线程和句柄；
|
||||||
|
- stop 与 SDK callback 并发；
|
||||||
|
- 最后一个流租约释放后不再发布；
|
||||||
|
- 多消费者使用独立游标；
|
||||||
|
- 环形队列覆盖和丢帧；
|
||||||
|
- epoch、sequence、关键帧和 codec generation；
|
||||||
|
- 设备断开、超时和重连。
|
||||||
|
|
||||||
|
无硬件参考测试：
|
||||||
|
|
||||||
|
- [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp)
|
||||||
|
- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp)
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cmake -S . -B build \
|
||||||
|
-DCMVR_ARCH=x86 \
|
||||||
|
-DBUILD_TESTING=ON \
|
||||||
|
-DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON
|
||||||
|
|
||||||
|
cmake --build build -j"$(nproc)"
|
||||||
|
|
||||||
|
ctest \
|
||||||
|
--test-dir build \
|
||||||
|
-R 'hikvision_camera_callback_test|media_source_hub_test' \
|
||||||
|
--output-on-failure
|
||||||
|
```
|
||||||
|
|
||||||
|
新测试必须在 `BUILD_TESTING` 下使用 `add_test()` 登记。只创建 executable 不会自动被 CTest 执行。
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] 抽象接口没有厂商 SDK 类型
|
||||||
|
- [ ] manager、外层配置和 backend ID 一致
|
||||||
|
- [ ] 新硬件配置默认关闭
|
||||||
|
- [ ] 生命周期支持重复调用
|
||||||
|
- [ ] stop 是同步发布屏障
|
||||||
|
- [ ] 状态读写使用同一把锁
|
||||||
|
- [ ] 每个实时流只有一个生产者
|
||||||
|
- [ ] 每个消费者使用独立游标
|
||||||
|
- [ ] 时间戳、sequence 和编码元数据完整
|
||||||
|
- [ ] Factory 和类别 CMake 均已接入
|
||||||
|
- [ ] 厂商运行库有安装规则
|
||||||
|
- [ ] 有无真实硬件的自动测试
|
||||||
|
- [ ] 平台能力变化已评估 Proto 和 service
|
||||||
@ -34,6 +34,23 @@ namespace cmvr::device {
|
|||||||
virtual bool start() { return true; }
|
virtual bool start() { return true; }
|
||||||
virtual bool stop() { return true; }
|
virtual bool stop() { return true; }
|
||||||
virtual bool update() { return true; }
|
virtual bool update() { return true; }
|
||||||
|
virtual bool executeJsonCommand(const std::string& request_json, std::string& response_json) {
|
||||||
|
response_json = R"({"success":false,"error_message":"JSON command unsupported"})";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This hook is sampled by DeviceManager while building heartbeats. It
|
||||||
|
// must be thread-safe and complete in bounded time while only copying
|
||||||
|
// in-memory state through atomics or a dedicated short-held state
|
||||||
|
// lock. Implementations must not perform device I/O, network requests,
|
||||||
|
// or wait on a lifecycle lock held across such I/O.
|
||||||
|
//
|
||||||
|
// The method is intentionally non-const because several legacy device
|
||||||
|
// categories expose non-const state getters. The returned object is a
|
||||||
|
// value and does not expose the device lifetime to callers.
|
||||||
|
virtual DeviceHealthSnapshot healthSnapshot() {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string id_; // 设备名称
|
std::string id_; // 设备名称
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
add_subdirectory(my_agv)
|
add_subdirectory(my_agv)
|
||||||
|
add_subdirectory(src1100)
|
||||||
|
|
||||||
add_library(agv INTERFACE)
|
add_library(agv INTERFACE)
|
||||||
|
|
||||||
@ -7,6 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
|||||||
target_link_libraries(agv
|
target_link_libraries(agv
|
||||||
INTERFACE
|
INTERFACE
|
||||||
cmvr_es::device::my_agv
|
cmvr_es::device::my_agv
|
||||||
|
cmvr_es::device::src1100_agv
|
||||||
cmvr_es::proto
|
cmvr_es::proto
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -6,32 +6,235 @@
|
|||||||
#define CMVR_ES_ABSTRACT_AGV_H
|
#define CMVR_ES_ABSTRACT_AGV_H
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/types/agv/agv_types.h"
|
||||||
#include "devices/abstract_device.h"
|
#include "devices/abstract_device.h"
|
||||||
|
|
||||||
namespace cmvr::device{
|
namespace cmvr::device {
|
||||||
class AbstractAGV: public AbstractDevice {
|
|
||||||
public:
|
|
||||||
AbstractAGV() = default;
|
|
||||||
~AbstractAGV() override=default;
|
|
||||||
|
|
||||||
DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
|
/**
|
||||||
virtual bool getState(AGVState &state) { return true; }
|
* @brief AGV/移动底盘设备抽象基类。
|
||||||
|
*
|
||||||
|
* 该接口只描述通用 AGV 能力。控制器特有的请求/响应字段应放在具体
|
||||||
|
* 驱动类中；当公共 API 需要扩展点时，可通过 AgvAdapterParams 传递。
|
||||||
|
*/
|
||||||
|
class AbstractAGV : public AbstractDevice {
|
||||||
|
public:
|
||||||
|
AbstractAGV() = default;
|
||||||
|
~AbstractAGV() override = default;
|
||||||
|
|
||||||
// navigation
|
DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
|
||||||
virtual bool eStop() { return true; }
|
|
||||||
virtual bool goHome() { return true; }
|
|
||||||
virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; }
|
|
||||||
virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; }
|
|
||||||
|
|
||||||
// map
|
/**
|
||||||
virtual bool initMap(float resolution, int width, int height) { return true; }
|
* @brief 获取 AGV 运行状态快照。
|
||||||
virtual bool updateMap() { return true; }
|
*/
|
||||||
virtual bool saveMap(const std::string& file_path) { return true; }
|
virtual AgvRuntimeState runtimeState() const { return {}; }
|
||||||
virtual bool loadMap(const std::string& file_path) { return true; }
|
|
||||||
|
|
||||||
protected:
|
/**
|
||||||
AGVState state_;
|
* @brief 获取当前导航任务状态。
|
||||||
};
|
*/
|
||||||
}
|
virtual AgvNavigationStatus navigationStatus() const { return {}; }
|
||||||
|
|
||||||
#endif //CMVR_ES_ABSTRACT_AGV_H
|
/**
|
||||||
|
* @brief 触发 AGV 急停行为。
|
||||||
|
*/
|
||||||
|
virtual AgvResult emergencyStop()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "emergencyStop not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 清除可恢复的 AGV 故障或告警。
|
||||||
|
*/
|
||||||
|
virtual AgvResult clearFault()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发起到世界/地图位姿的导航任务。
|
||||||
|
*/
|
||||||
|
virtual AgvResult navigateToPose(
|
||||||
|
const math::Pose2d& pose,
|
||||||
|
const AgvMotionOptions& options = {},
|
||||||
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{})
|
||||||
|
{
|
||||||
|
(void)pose;
|
||||||
|
(void)options;
|
||||||
|
(void)adapter_params;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToPose not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发起到指定地图站点的导航任务。
|
||||||
|
*/
|
||||||
|
virtual AgvResult navigateToStation(
|
||||||
|
const std::string& station_id,
|
||||||
|
const AgvMotionOptions& options = {},
|
||||||
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{})
|
||||||
|
{
|
||||||
|
(void)station_id;
|
||||||
|
(void)options;
|
||||||
|
(void)adapter_params;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToStation not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发起显式站点到站点路径导航任务。
|
||||||
|
*/
|
||||||
|
virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
|
||||||
|
{
|
||||||
|
(void)path;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 暂停当前导航任务，如果设备支持。
|
||||||
|
*/
|
||||||
|
virtual AgvResult pauseNavigation()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "pauseNavigation not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 恢复已暂停的导航任务，如果设备支持。
|
||||||
|
*/
|
||||||
|
virtual AgvResult resumeNavigation()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "resumeNavigation not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 取消当前导航任务，如果设备支持。
|
||||||
|
*/
|
||||||
|
virtual AgvResult cancelNavigation()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "cancelNavigation not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 向 AGV 下发低层速度控制指令。
|
||||||
|
*
|
||||||
|
* 该接口不同于导航命令。具体实现应明确速度控制在导航过程中是中断
|
||||||
|
* 导航、与导航共存，还是被拒绝执行。
|
||||||
|
*/
|
||||||
|
virtual AgvResult setVelocity(const AgvVelocity& velocity)
|
||||||
|
{
|
||||||
|
(void)velocity;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "setVelocity not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 通过下发零速度停止低层速度控制。
|
||||||
|
*
|
||||||
|
* 该接口不表示取消正在执行的导航任务；取消导航请使用
|
||||||
|
* cancelNavigation()。
|
||||||
|
*/
|
||||||
|
virtual AgvResult stopVelocityControl()
|
||||||
|
{
|
||||||
|
return setVelocity(AgvVelocity{});
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 查询 AGV 可用地图名称列表。
|
||||||
|
*/
|
||||||
|
virtual AgvResult listMaps(std::vector<std::string>& maps) const
|
||||||
|
{
|
||||||
|
(void)maps;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listMaps not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 查询当前活动地图中的站点列表。
|
||||||
|
*/
|
||||||
|
virtual AgvResult listStations(std::vector<AgvStation>& stations) const
|
||||||
|
{
|
||||||
|
(void)stations;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listStations not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 切换当前活动地图。
|
||||||
|
*/
|
||||||
|
virtual AgvResult switchMap(const std::string& map_name)
|
||||||
|
{
|
||||||
|
(void)map_name;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "switchMap not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 按名称上传或替换地图。
|
||||||
|
*/
|
||||||
|
virtual AgvResult uploadMap(const std::string& map_name, const std::string& content)
|
||||||
|
{
|
||||||
|
(void)map_name;
|
||||||
|
(void)content;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "uploadMap not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 按名称下载地图内容。
|
||||||
|
*/
|
||||||
|
virtual AgvResult downloadMap(const std::string& map_name, std::string& content) const
|
||||||
|
{
|
||||||
|
(void)map_name;
|
||||||
|
(void)content;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "downloadMap not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 开始扫图/建图。
|
||||||
|
*/
|
||||||
|
virtual AgvResult startMapping(const AgvMappingOptions& options = {})
|
||||||
|
{
|
||||||
|
(void)options;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "startMapping not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 从指定下标开始获取厂商原始扫图数据。
|
||||||
|
*
|
||||||
|
* 该接口主要保留给具体驱动内部使用。对外 gRPC 地图流应优先使用
|
||||||
|
* getUnifiedMapUpdate()，避免把厂商文件格式暴露给上位机。
|
||||||
|
*/
|
||||||
|
virtual AgvResult getMappingData(int start_index, AgvMappingData& data) const
|
||||||
|
{
|
||||||
|
(void)start_index;
|
||||||
|
(void)data;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getMappingData not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 获取统一地图更新。
|
||||||
|
*
|
||||||
|
* after_sequence 为 0 时通常返回最近可用的全量快照；大于 0 时返回
|
||||||
|
* 指定序号之后的下一条更新。如果当前没有新地图，具体实现可在
|
||||||
|
* options.wait_timeout_ms 内等待后台更新线程写入缓存。
|
||||||
|
*/
|
||||||
|
virtual AgvResult getUnifiedMapUpdate(
|
||||||
|
std::uint64_t after_sequence,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
AgvUnifiedMapUpdate& update) const
|
||||||
|
{
|
||||||
|
(void)after_sequence;
|
||||||
|
(void)options;
|
||||||
|
(void)update;
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getUnifiedMapUpdate not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 停止扫图/建图。
|
||||||
|
*/
|
||||||
|
virtual AgvResult stopMapping()
|
||||||
|
{
|
||||||
|
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "stopMapping not implemented");
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::device
|
||||||
|
|
||||||
|
#endif // CMVR_ES_ABSTRACT_AGV_H
|
||||||
|
|||||||
@ -7,6 +7,7 @@
|
|||||||
#include "common/base/logging/logger.h"
|
#include "common/base/logging/logger.h"
|
||||||
#include "devices/agv/abstract_agv.h"
|
#include "devices/agv/abstract_agv.h"
|
||||||
#include "devices/agv/my_agv/include/my_agv.h"
|
#include "devices/agv/my_agv/include/my_agv.h"
|
||||||
|
#include "devices/agv/src1100/include/src1100_agv.h"
|
||||||
|
|
||||||
namespace cmvr::device {
|
namespace cmvr::device {
|
||||||
|
|
||||||
@ -30,6 +31,16 @@ public:
|
|||||||
backend.set_id(cfg.id());
|
backend.set_id(cfg.id());
|
||||||
return std::make_shared<MyAgv>(backend);
|
return std::make_shared<MyAgv>(backend);
|
||||||
}
|
}
|
||||||
|
case config::AGVDeviceConfig::kSrc1100Agv:
|
||||||
|
{
|
||||||
|
if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) {
|
||||||
|
CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id();
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
auto backend = cfg.src1100_agv();
|
||||||
|
backend.set_id(cfg.id());
|
||||||
|
return std::make_shared<Src1100Agv>(backend);
|
||||||
|
}
|
||||||
|
|
||||||
case config::AGVDeviceConfig::BACKEND_NOT_SET:
|
case config::AGVDeviceConfig::BACKEND_NOT_SET:
|
||||||
default:
|
default:
|
||||||
|
|||||||
@ -20,15 +20,13 @@ public:
|
|||||||
bool stop() override;
|
bool stop() override;
|
||||||
bool update() override;
|
bool update() override;
|
||||||
|
|
||||||
bool getState(AGVState& state) override;
|
AgvRuntimeState runtimeState() const override;
|
||||||
bool eStop() override;
|
AgvResult emergencyStop() override;
|
||||||
bool goHome() override;
|
AgvResult navigateToPose(
|
||||||
bool moveto(math::Pose2d& location, double speed_ratio) override;
|
const math::Pose2d& pose,
|
||||||
bool setVelocity(math::Vec3 linear, math::Vec3 angular) override;
|
const AgvMotionOptions& options = {},
|
||||||
bool initMap(float resolution, int width, int height) override;
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||||
bool updateMap() override;
|
AgvResult setVelocity(const AgvVelocity& velocity) override;
|
||||||
bool saveMap(const std::string& file_path) override;
|
|
||||||
bool loadMap(const std::string& file_path) override;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
config::MyAgvConfig config_;
|
config::MyAgvConfig config_;
|
||||||
|
|||||||
@ -27,49 +27,27 @@ bool MyAgv::update()
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MyAgv::getState(AGVState&)
|
AgvRuntimeState MyAgv::runtimeState() const
|
||||||
{
|
{
|
||||||
return true;
|
return {};
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MyAgv::eStop()
|
AgvResult MyAgv::emergencyStop()
|
||||||
{
|
{
|
||||||
return true;
|
return AgvResult::success();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MyAgv::goHome()
|
AgvResult MyAgv::navigateToPose(
|
||||||
|
const math::Pose2d&,
|
||||||
|
const AgvMotionOptions&,
|
||||||
|
const AgvAdapterParams&)
|
||||||
{
|
{
|
||||||
return true;
|
return AgvResult::success();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MyAgv::moveto(math::Pose2d&, double)
|
AgvResult MyAgv::setVelocity(const AgvVelocity&)
|
||||||
{
|
{
|
||||||
return true;
|
return AgvResult::success();
|
||||||
}
|
|
||||||
|
|
||||||
bool MyAgv::setVelocity(math::Vec3, math::Vec3)
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool MyAgv::initMap(float, int, int)
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool MyAgv::updateMap()
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool MyAgv::saveMap(const std::string&)
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool MyAgv::loadMap(const std::string&)
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace cmvr::device
|
} // namespace cmvr::device
|
||||||
|
|||||||
12
cmvr-es/devices/agv/src1100/CMakeLists.txt
Normal file
12
cmvr-es/devices/agv/src1100/CMakeLists.txt
Normal file
@ -0,0 +1,12 @@
|
|||||||
|
add_library(src1100_agv SHARED src/src1100_agv.cpp)
|
||||||
|
|
||||||
|
target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||||
|
|
||||||
|
target_link_libraries(src1100_agv
|
||||||
|
PUBLIC
|
||||||
|
cmvr_es::proto
|
||||||
|
jsoncpp
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv)
|
||||||
|
install(TARGETS src1100_agv LIBRARY DESTINATION lib)
|
||||||
179
cmvr-es/devices/agv/src1100/include/src1100_agv.h
Normal file
179
cmvr-es/devices/agv/src1100/include/src1100_agv.h
Normal file
@ -0,0 +1,179 @@
|
|||||||
|
#ifndef CMVR_ES_SRC1100_AGV_H
|
||||||
|
#define CMVR_ES_SRC1100_AGV_H
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <deque>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <json/json.h>
|
||||||
|
|
||||||
|
#include "cmvr/config/agv_config/agv_config.pb.h"
|
||||||
|
#include "devices/agv/abstract_agv.h"
|
||||||
|
|
||||||
|
namespace cmvr::device {
|
||||||
|
|
||||||
|
class Src1100Agv final : public AbstractAGV {
|
||||||
|
public:
|
||||||
|
explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
|
||||||
|
~Src1100Agv() override;
|
||||||
|
|
||||||
|
std::string typeName() const override { return "Src1100Agv"; }
|
||||||
|
|
||||||
|
bool init() override;
|
||||||
|
bool start() override;
|
||||||
|
bool stop() override;
|
||||||
|
bool update() override;
|
||||||
|
|
||||||
|
AgvRuntimeState runtimeState() const override;
|
||||||
|
AgvNavigationStatus navigationStatus() const override;
|
||||||
|
|
||||||
|
AgvResult emergencyStop() override;
|
||||||
|
AgvResult clearFault() override;
|
||||||
|
|
||||||
|
AgvResult navigateToPose(
|
||||||
|
const math::Pose2d& pose,
|
||||||
|
const AgvMotionOptions& options = {},
|
||||||
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||||
|
AgvResult navigateToStation(
|
||||||
|
const std::string& station_id,
|
||||||
|
const AgvMotionOptions& options = {},
|
||||||
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||||
|
AgvResult followPath(const std::vector<AgvPathSegment>& path) override;
|
||||||
|
AgvResult pauseNavigation() override;
|
||||||
|
AgvResult resumeNavigation() override;
|
||||||
|
AgvResult cancelNavigation() override;
|
||||||
|
|
||||||
|
AgvResult setVelocity(const AgvVelocity& velocity) override;
|
||||||
|
|
||||||
|
AgvResult listMaps(std::vector<std::string>& maps) const override;
|
||||||
|
AgvResult listStations(std::vector<AgvStation>& stations) const override;
|
||||||
|
AgvResult switchMap(const std::string& map_name) override;
|
||||||
|
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
|
||||||
|
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
|
||||||
|
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
|
||||||
|
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
|
||||||
|
AgvResult getUnifiedMapUpdate(
|
||||||
|
std::uint64_t after_sequence,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
AgvUnifiedMapUpdate& update) const override;
|
||||||
|
AgvResult stopMapping() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Ports {
|
||||||
|
int status{19204};
|
||||||
|
int control{19205};
|
||||||
|
int navigation{19206};
|
||||||
|
int config{19207};
|
||||||
|
int other{19210};
|
||||||
|
int push{19301};
|
||||||
|
};
|
||||||
|
|
||||||
|
AgvResult connect_();
|
||||||
|
AgvResult disconnect_();
|
||||||
|
AgvResult connectSocket_(int& sock, int port);
|
||||||
|
AgvResult ensureOtherSocket_();
|
||||||
|
void closeSocket_(int& sock) const;
|
||||||
|
bool connected_() const;
|
||||||
|
|
||||||
|
AgvResult sendCommand_(int sock,
|
||||||
|
std::uint16_t command,
|
||||||
|
const Json::Value& payload,
|
||||||
|
Json::Value* response) const;
|
||||||
|
AgvResult sendCommandRaw_(int sock,
|
||||||
|
std::uint16_t command,
|
||||||
|
const Json::Value& payload,
|
||||||
|
std::string* response_payload) const;
|
||||||
|
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
|
||||||
|
AgvResult configurePush_();
|
||||||
|
void startPushThread_();
|
||||||
|
void stopPushThread_();
|
||||||
|
void pushLoop_();
|
||||||
|
AgvRuntimeState queryRuntimeState_() const;
|
||||||
|
void updateCachedRuntimeState_(const Json::Value& payload);
|
||||||
|
void startMapUpdateThread_();
|
||||||
|
void stopMapUpdateThread_();
|
||||||
|
void mapUpdateLoop_();
|
||||||
|
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
|
||||||
|
AgvResult parseMapFileToUpdates_(
|
||||||
|
const std::string& file_name,
|
||||||
|
const std::string& content,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||||
|
AgvResult parseSrc1100MapArchive_(
|
||||||
|
const std::string& file_name,
|
||||||
|
const std::string& content,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||||
|
AgvResult parseSrc1100Map2D_(
|
||||||
|
const std::string& file_name,
|
||||||
|
const std::string& content,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
AgvUnifiedMapUpdate& update) const;
|
||||||
|
AgvResult parseSrc1100Map3D_(
|
||||||
|
const std::string& file_name,
|
||||||
|
const std::string& content,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
AgvUnifiedMapUpdate& update) const;
|
||||||
|
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
|
||||||
|
bool findCachedMapUpdate_(
|
||||||
|
std::uint64_t after_sequence,
|
||||||
|
const AgvMapStreamOptions& options,
|
||||||
|
AgvUnifiedMapUpdate& update) const;
|
||||||
|
bool mapUpdateMatches_(
|
||||||
|
const AgvUnifiedMapUpdate& update,
|
||||||
|
const AgvMapStreamOptions& options) const;
|
||||||
|
|
||||||
|
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
|
||||||
|
static std::string toJsonString_(const Json::Value& value);
|
||||||
|
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
|
||||||
|
static std::string extractJson_(const std::string& raw);
|
||||||
|
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
|
||||||
|
static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
|
||||||
|
static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
|
||||||
|
static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
|
||||||
|
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
|
||||||
|
static AgvResult resultFromResponse_(const Json::Value& response);
|
||||||
|
|
||||||
|
config::Src1100AgvConfig config_;
|
||||||
|
std::string ip_;
|
||||||
|
int recv_timeout_ms_{1000};
|
||||||
|
Ports ports_;
|
||||||
|
bool state_push_enabled_{false};
|
||||||
|
bool map_update_enabled_{false};
|
||||||
|
int map_update_interval_ms_{1000};
|
||||||
|
std::size_t map_update_history_size_{8};
|
||||||
|
|
||||||
|
mutable std::mutex mutex_;
|
||||||
|
int sock_status_{-1};
|
||||||
|
int sock_control_{-1};
|
||||||
|
int sock_navigation_{-1};
|
||||||
|
int sock_config_{-1};
|
||||||
|
int sock_other_{-1};
|
||||||
|
int sock_push_{-1};
|
||||||
|
std::string last_error_;
|
||||||
|
|
||||||
|
std::atomic<bool> push_running_{false};
|
||||||
|
std::thread push_thread_;
|
||||||
|
mutable std::mutex runtime_state_mutex_;
|
||||||
|
AgvRuntimeState cached_runtime_state_;
|
||||||
|
bool cached_runtime_state_valid_{false};
|
||||||
|
|
||||||
|
mutable std::atomic<bool> map_update_running_{false};
|
||||||
|
mutable std::thread map_update_thread_;
|
||||||
|
mutable std::mutex map_update_mutex_;
|
||||||
|
mutable std::condition_variable map_update_cv_;
|
||||||
|
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
|
||||||
|
mutable std::uint64_t map_sequence_{0};
|
||||||
|
mutable int next_mapping_index_{0};
|
||||||
|
mutable std::size_t last_map_content_hash_{0};
|
||||||
|
mutable std::string map_session_id_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::device
|
||||||
|
|
||||||
|
#endif // CMVR_ES_SRC1100_AGV_H
|
||||||
1898
cmvr-es/devices/agv/src1100/src/src1100_agv.cpp
Normal file
1898
cmvr-es/devices/agv/src1100/src/src1100_agv.cpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -1,13 +1,36 @@
|
|||||||
add_library(aubo_arm SHARED
|
add_library(aubo_arm SHARED
|
||||||
src/aubo_arm.cpp
|
aubo_arm.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
|
set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
|
||||||
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
|
set(AUBO_SDK_INCLUDE_DIR ${AUBO_SDK_ROOT}/include)
|
||||||
|
set(AUBO_SDK_LIB_DIR ${AUBO_SDK_ROOT}/lib)
|
||||||
|
|
||||||
if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
|
if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake")
|
||||||
|
list(APPEND CMAKE_PREFIX_PATH "${AUBO_SDK_LIB_DIR}/cmake")
|
||||||
|
find_package(Qt5Core QUIET)
|
||||||
|
if (NOT Qt5Core_FOUND AND NOT TARGET Qt5::Core)
|
||||||
|
find_library(QT5_CORE_LIBRARY
|
||||||
|
NAMES Qt5Core libQt5Core.so.5
|
||||||
|
PATHS /lib /usr/lib /usr/local/lib /lib/x86_64-linux-gnu /usr/lib/x86_64-linux-gnu
|
||||||
|
)
|
||||||
|
if (QT5_CORE_LIBRARY)
|
||||||
|
add_library(Qt5::Core UNKNOWN IMPORTED)
|
||||||
|
set_target_properties(Qt5::Core PROPERTIES
|
||||||
|
IMPORTED_LOCATION "${QT5_CORE_LIBRARY}"
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
|
find_package(aubo_sdk REQUIRED CONFIG PATHS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk" NO_DEFAULT_PATH)
|
||||||
|
set_target_properties(aubo_sdk::aubo_sdk aubo_sdk::robot_proxy PROPERTIES
|
||||||
|
MAP_IMPORTED_CONFIG_DEBUG Release
|
||||||
|
)
|
||||||
|
target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
|
||||||
|
target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
|
||||||
|
target_link_libraries(aubo_arm PRIVATE aubo_sdk::aubo_sdk)
|
||||||
|
elseif (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
|
||||||
target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
|
target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
|
||||||
target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
|
target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
|
||||||
if (EXISTS "${AUBO_SDK_LIB_DIR}")
|
if (EXISTS "${AUBO_SDK_LIB_DIR}")
|
||||||
|
|||||||
1065
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
Normal file
1065
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -29,7 +29,7 @@ public:
|
|||||||
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
|
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
|
||||||
RobotMode getRobotMode() const override;
|
RobotMode getRobotMode() const override;
|
||||||
SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
|
SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
|
||||||
ControlMode getControlMode() const override { return ControlMode::Position; }
|
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
|
||||||
|
|
||||||
Result torqueOn() override;
|
Result torqueOn() override;
|
||||||
Result torqueOff() override;
|
Result torqueOff() override;
|
||||||
@ -98,8 +98,10 @@ private:
|
|||||||
std::string username_;
|
std::string username_;
|
||||||
std::string password_;
|
std::string password_;
|
||||||
double speed_scaling_{1.0};
|
double speed_scaling_{1.0};
|
||||||
|
ServoOptions servo_options_;
|
||||||
std::atomic<bool> connected_{false};
|
std::atomic<bool> connected_{false};
|
||||||
std::atomic<bool> busy_{false};
|
std::atomic<bool> busy_{false};
|
||||||
|
std::atomic<bool> servo_mode_{false};
|
||||||
bool emergency_stopped_{false};
|
bool emergency_stopped_{false};
|
||||||
mutable std::mutex mutex_;
|
mutable std::mutex mutex_;
|
||||||
|
|
||||||
@ -1,575 +0,0 @@
|
|||||||
#include "devices/arm/aubo_arm/include/aubo_arm.h"
|
|
||||||
|
|
||||||
#include <algorithm>
|
|
||||||
#include <chrono>
|
|
||||||
#include <exception>
|
|
||||||
#include <thread>
|
|
||||||
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
#include "aubo_sdk/rpc.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
struct BusyGuard {
|
|
||||||
std::atomic<bool>& busy;
|
|
||||||
~BusyGuard() { busy.store(false); }
|
|
||||||
};
|
|
||||||
|
|
||||||
std::vector<std::string> defaultJointNames(const std::size_t dof)
|
|
||||||
{
|
|
||||||
std::vector<std::string> names;
|
|
||||||
names.reserve(dof);
|
|
||||||
for (std::size_t i = 0; i < dof; ++i) {
|
|
||||||
names.push_back("joint_" + std::to_string(i + 1));
|
|
||||||
}
|
|
||||||
return names;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string vendorBrandName(const config::VendorRobotArmBrand brand)
|
|
||||||
{
|
|
||||||
switch (brand) {
|
|
||||||
case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM:
|
|
||||||
return "AuboARM";
|
|
||||||
case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN:
|
|
||||||
default:
|
|
||||||
return "Unknown";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
struct AuboArm::SdkState {
|
|
||||||
std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
|
|
||||||
};
|
|
||||||
#endif
|
|
||||||
|
|
||||||
AuboArm::AuboArm(const config::RobotArmConfig& cfg)
|
|
||||||
: cfg_(cfg)
|
|
||||||
{
|
|
||||||
id_ = cfg.id();
|
|
||||||
if (cfg.has_vendor()) {
|
|
||||||
vendor_cfg_ = cfg.vendor();
|
|
||||||
}
|
|
||||||
|
|
||||||
ip_ = vendor_cfg_.ip();
|
|
||||||
port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004;
|
|
||||||
username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username();
|
|
||||||
password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password();
|
|
||||||
|
|
||||||
const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
|
|
||||||
model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model();
|
|
||||||
model_.manufacturer = vendorBrandName(vendor_cfg_.brand());
|
|
||||||
model_.dof = dof;
|
|
||||||
model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end());
|
|
||||||
if (model_.joint_names.empty()) {
|
|
||||||
model_.joint_names = defaultJointNames(dof);
|
|
||||||
}
|
|
||||||
if (model_.joint_names.size() != dof) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_;
|
|
||||||
model_.joint_names = defaultJointNames(dof);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
AuboArm::~AuboArm()
|
|
||||||
{
|
|
||||||
(void)disconnect();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AuboArm::init()
|
|
||||||
{
|
|
||||||
if (ip_.empty()) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto result = connect(ip_, port_);
|
|
||||||
if (!result.ok()) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AuboArm::stop()
|
|
||||||
{
|
|
||||||
return stopMotion().ok();
|
|
||||||
}
|
|
||||||
|
|
||||||
ArmState AuboArm::getRobotState() const
|
|
||||||
{
|
|
||||||
ArmState state;
|
|
||||||
state.connected = connected_.load();
|
|
||||||
state.powered_on = state.connected;
|
|
||||||
state.brake_released = state.connected;
|
|
||||||
state.moving = busy_.load();
|
|
||||||
state.robot_mode = getRobotMode();
|
|
||||||
state.safety_mode = getSafetyMode();
|
|
||||||
state.control_mode = getControlMode();
|
|
||||||
state.emergency_stopped = emergency_stopped_;
|
|
||||||
state.speed_scaling = speed_scaling_;
|
|
||||||
state.actual_joint_state = getJointState();
|
|
||||||
state.target_joint_state = state.actual_joint_state;
|
|
||||||
return state;
|
|
||||||
}
|
|
||||||
|
|
||||||
JointGroupState AuboArm::getJointState() const
|
|
||||||
{
|
|
||||||
JointGroupState state;
|
|
||||||
state.position.assign(model_.dof, 0.0);
|
|
||||||
state.velocity.assign(model_.dof, 0.0);
|
|
||||||
state.effort.assign(model_.dof, 0.0);
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
||||||
return state;
|
|
||||||
}
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty";
|
|
||||||
return state;
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (!robot_interface) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null";
|
|
||||||
return state;
|
|
||||||
}
|
|
||||||
const auto robot_state = robot_interface->getRobotState();
|
|
||||||
const auto positions = robot_state->getJointPositions();
|
|
||||||
const auto velocities = robot_state->getJointSpeeds();
|
|
||||||
const auto n = std::min<std::size_t>(model_.dof, positions.size());
|
|
||||||
for (std::size_t i = 0; i < n; ++i) {
|
|
||||||
state.position[i] = positions[i];
|
|
||||||
}
|
|
||||||
const auto vn = std::min<std::size_t>(model_.dof, velocities.size());
|
|
||||||
for (std::size_t i = 0; i < vn; ++i) {
|
|
||||||
state.velocity[i] = velocities[i];
|
|
||||||
}
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
return state;
|
|
||||||
}
|
|
||||||
|
|
||||||
CartesianPose AuboArm::getTcpPose(FrameType frame) const
|
|
||||||
{
|
|
||||||
(void)frame;
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
RobotMode AuboArm::getRobotMode() const
|
|
||||||
{
|
|
||||||
if (!connected_.load()) {
|
|
||||||
return RobotMode::Disconnected;
|
|
||||||
}
|
|
||||||
if (emergency_stopped_) {
|
|
||||||
return RobotMode::Stopped;
|
|
||||||
}
|
|
||||||
return busy_.load() ? RobotMode::Running : RobotMode::Idle;
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::torqueOn()
|
|
||||||
{
|
|
||||||
const auto ready = ensureConnected_("torqueOn");
|
|
||||||
if (!ready.ok()) {
|
|
||||||
return ready;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (!robot_interface) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
||||||
}
|
|
||||||
|
|
||||||
double mass = 0.0;
|
|
||||||
std::vector<double> cog(3, 0.0);
|
|
||||||
std::vector<double> aom(3, 0.0);
|
|
||||||
std::vector<double> inertia(6, 0.0);
|
|
||||||
robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
|
|
||||||
|
|
||||||
if (robot_interface->getRobotState()->getRobotModeType() !=
|
|
||||||
arcs::common_interface::RobotModeType::Running) {
|
|
||||||
robot_interface->getRobotManage()->poweron();
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(200));
|
|
||||||
robot_interface->getRobotManage()->startup();
|
|
||||||
}
|
|
||||||
emergency_stopped_ = false;
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return unsupported_("torqueOn");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::torqueOff()
|
|
||||||
{
|
|
||||||
const auto ready = ensureConnected_("torqueOff");
|
|
||||||
if (!ready.ok()) {
|
|
||||||
return ready;
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (!robot_interface) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
||||||
}
|
|
||||||
robot_interface->getRobotManage()->poweroff();
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return unsupported_("torqueOff");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::calibrateZeroQ(const std::string& joint_name)
|
|
||||||
{
|
|
||||||
(void)joint_name;
|
|
||||||
return unsupported_("calibrateZeroQ");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::emergencyStop()
|
|
||||||
{
|
|
||||||
emergency_stopped_ = true;
|
|
||||||
return stopMotion();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::setSpeedScaling(const double scaling)
|
|
||||||
{
|
|
||||||
if (scaling < 0.0 || scaling > 1.0) {
|
|
||||||
return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]");
|
|
||||||
}
|
|
||||||
speed_scaling_ = scaling;
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
|
|
||||||
{
|
|
||||||
std::string error;
|
|
||||||
if (!validDof_(target.position.size(), error)) {
|
|
||||||
return Result::failure(ArmErrorCode::InvalidDof, error);
|
|
||||||
}
|
|
||||||
const auto ready = ensureConnected_("moveJ");
|
|
||||||
if (!ready.ok()) {
|
|
||||||
return ready;
|
|
||||||
}
|
|
||||||
if (busy_.exchange(true)) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
||||||
}
|
|
||||||
BusyGuard busy_guard{busy_};
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (!robot_interface) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
||||||
}
|
|
||||||
robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
|
|
||||||
robot_interface->getMotionControl()->moveJoint(
|
|
||||||
target.position,
|
|
||||||
options.acceleration > 0.0 ? options.acceleration : 0.5,
|
|
||||||
options.velocity > 0.0 ? options.velocity : 0.5,
|
|
||||||
options.blend_radius,
|
|
||||||
0);
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return unsupported_("moveJ");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration)
|
|
||||||
{
|
|
||||||
(void)velocity;
|
|
||||||
(void)acceleration;
|
|
||||||
(void)duration;
|
|
||||||
return unsupported_("speedJ");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::stopJ(double acceleration)
|
|
||||||
{
|
|
||||||
(void)acceleration;
|
|
||||||
return stopMotion();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
|
|
||||||
{
|
|
||||||
(void)frame;
|
|
||||||
const auto ready = ensureConnected_("moveL");
|
|
||||||
if (!ready.ok()) {
|
|
||||||
return ready;
|
|
||||||
}
|
|
||||||
if (busy_.exchange(true)) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
||||||
}
|
|
||||||
BusyGuard busy_guard{busy_};
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (!robot_interface) {
|
|
||||||
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
||||||
}
|
|
||||||
robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
|
|
||||||
std::vector<double> tcp_offset(6, 0.0);
|
|
||||||
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
|
|
||||||
std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
|
|
||||||
robot_interface->getMotionControl()->moveLine(
|
|
||||||
pose,
|
|
||||||
options.acceleration > 0.0 ? options.acceleration : 0.5,
|
|
||||||
options.velocity > 0.0 ? options.velocity : 0.25,
|
|
||||||
options.blend_radius,
|
|
||||||
0);
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return unsupported_("moveL");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
|
|
||||||
{
|
|
||||||
(void)velocity;
|
|
||||||
(void)acceleration;
|
|
||||||
(void)duration;
|
|
||||||
(void)frame;
|
|
||||||
return unsupported_("speedL");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::stopL(std::optional<double> acceleration)
|
|
||||||
{
|
|
||||||
(void)acceleration;
|
|
||||||
return stopMotion();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::stopMotion()
|
|
||||||
{
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
try {
|
|
||||||
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
||||||
if (robot_names.empty()) {
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
||||||
if (robot_interface) {
|
|
||||||
robot_interface->getMotionControl()->stopMove();
|
|
||||||
}
|
|
||||||
busy_.store(false);
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
busy_.store(false);
|
|
||||||
return Result::success();
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::startServoMode(const ServoOptions& options)
|
|
||||||
{
|
|
||||||
(void)options;
|
|
||||||
return unsupported_("startServoMode");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::servoJ(const JointPositionCommand& target)
|
|
||||||
{
|
|
||||||
(void)target;
|
|
||||||
return unsupported_("servoJ");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::servoL(const CartesianPose& target, FrameType frame)
|
|
||||||
{
|
|
||||||
(void)target;
|
|
||||||
(void)frame;
|
|
||||||
return unsupported_("servoL");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity)
|
|
||||||
{
|
|
||||||
(void)velocity;
|
|
||||||
return unsupported_("servoSpeedJ");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame)
|
|
||||||
{
|
|
||||||
(void)velocity;
|
|
||||||
(void)frame;
|
|
||||||
return unsupported_("servoSpeedL");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::stopServoMode()
|
|
||||||
{
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::connect(const std::string& ip, const int port)
|
|
||||||
{
|
|
||||||
if (connected_.load()) {
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
if (ip.empty()) {
|
|
||||||
return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty");
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
sdk_ = std::make_unique<SdkState>();
|
|
||||||
sdk_->rpc_client = std::make_shared<arcs::aubo_sdk::RpcClient>();
|
|
||||||
sdk_->rpc_client->setRequestTimeout(1000);
|
|
||||||
sdk_->rpc_client->connect(ip, port > 0 ? port : 30004);
|
|
||||||
sdk_->rpc_client->login(username_, password_);
|
|
||||||
ip_ = ip;
|
|
||||||
port_ = port > 0 ? port : 30004;
|
|
||||||
connected_.store(true);
|
|
||||||
return Result::success();
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
sdk_.reset();
|
|
||||||
connected_.store(false);
|
|
||||||
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
||||||
std::string("[AuboArm] connect failed: ") + e.what());
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
(void)port;
|
|
||||||
return Result::failure(ArmErrorCode::UnsupportedCommand,
|
|
||||||
"[AuboArm] Aubo SDK is not available in this build");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::disconnect()
|
|
||||||
{
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
try {
|
|
||||||
if (sdk_ && sdk_->rpc_client) {
|
|
||||||
sdk_->rpc_client->logout();
|
|
||||||
sdk_->rpc_client->disconnect();
|
|
||||||
}
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what();
|
|
||||||
}
|
|
||||||
sdk_.reset();
|
|
||||||
#endif
|
|
||||||
connected_.store(false);
|
|
||||||
busy_.store(false);
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::shutdown()
|
|
||||||
{
|
|
||||||
(void)stopMotion();
|
|
||||||
return disconnect();
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::loadProgram(const std::string& program_name)
|
|
||||||
{
|
|
||||||
(void)program_name;
|
|
||||||
return unsupported_("loadProgram");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::playProgram()
|
|
||||||
{
|
|
||||||
return unsupported_("playProgram");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::pauseProgram()
|
|
||||||
{
|
|
||||||
return unsupported_("pauseProgram");
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::stopProgram()
|
|
||||||
{
|
|
||||||
return unsupported_("stopProgram");
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<double> AuboArm::ik(const std::string& base_link,
|
|
||||||
const std::string& ee_link,
|
|
||||||
const CartesianPose& pose)
|
|
||||||
{
|
|
||||||
(void)base_link;
|
|
||||||
(void)ee_link;
|
|
||||||
(void)pose;
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] ik is not implemented";
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link)
|
|
||||||
{
|
|
||||||
(void)base_link;
|
|
||||||
(void)ee_link;
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] fk(base,ee) is not implemented";
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
CartesianPose AuboArm::fk(bool is_tcp)
|
|
||||||
{
|
|
||||||
(void)is_tcp;
|
|
||||||
CMVR_LOG(ERROR) << "[AuboArm] fk is not implemented";
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::unsupported_(const std::string& name) const
|
|
||||||
{
|
|
||||||
const std::string message = "[AuboArm] " + name + " is not implemented";
|
|
||||||
CMVR_LOG(ERROR) << message;
|
|
||||||
return Result::failure(ArmErrorCode::UnsupportedCommand, message);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AuboArm::validDof_(const std::size_t size, std::string& error) const
|
|
||||||
{
|
|
||||||
if (size != model_.dof) {
|
|
||||||
error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) +
|
|
||||||
", actual=" + std::to_string(size);
|
|
||||||
CMVR_LOG(ERROR) << error;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
Result AuboArm::ensureConnected_(const std::string& context) const
|
|
||||||
{
|
|
||||||
if (!connected_.load()) {
|
|
||||||
return Result::failure(ArmErrorCode::NotConnected,
|
|
||||||
"[AuboArm] " + context + " failed: arm is not connected");
|
|
||||||
}
|
|
||||||
#if defined(CMVR_HAS_AUBO_SDK)
|
|
||||||
if (!sdk_ || !sdk_->rpc_client) {
|
|
||||||
return Result::failure(ArmErrorCode::NotConnected,
|
|
||||||
"[AuboArm] " + context + " failed: SDK client is null");
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
return Result::success();
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
@ -117,6 +117,7 @@ bool HuayanRobot::init()
|
|||||||
CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message;
|
CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
setSpeedScaling(1);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -6,7 +6,7 @@
|
|||||||
|
|
||||||
#include "cmvr/config/arm_config/arm_config.pb.h"
|
#include "cmvr/config/arm_config/arm_config.pb.h"
|
||||||
#include "common/base/logging/logger.h"
|
#include "common/base/logging/logger.h"
|
||||||
#include "devices/arm/aubo_arm/include/aubo_arm.h"
|
#include "devices/arm/aubo_arm/aubo_arm.h"
|
||||||
#include "devices/arm/huayan_arm/huayan_arm.h"
|
#include "devices/arm/huayan_arm/huayan_arm.h"
|
||||||
#include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
|
#include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
|
||||||
|
|
||||||
|
|||||||
@ -3,7 +3,7 @@ add_subdirectory(common)
|
|||||||
add_subdirectory(uvc_camera)
|
add_subdirectory(uvc_camera)
|
||||||
add_subdirectory(realsense_camera)
|
add_subdirectory(realsense_camera)
|
||||||
add_subdirectory(mujoco_camera)
|
add_subdirectory(mujoco_camera)
|
||||||
|
add_subdirectory(hikvision_camera)
|
||||||
add_library(camera INTERFACE)
|
add_library(camera INTERFACE)
|
||||||
|
|
||||||
target_include_directories(camera INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(camera INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
@ -14,6 +14,7 @@ target_link_libraries(camera
|
|||||||
cmvr_es::device::realsense_camera
|
cmvr_es::device::realsense_camera
|
||||||
cmvr_es::device::mujoco_camera
|
cmvr_es::device::mujoco_camera
|
||||||
cmvr_es::device::camera_stream_encoder
|
cmvr_es::device::camera_stream_encoder
|
||||||
|
cmvr_es::device::hikvision_camera
|
||||||
cmvr_es::proto
|
cmvr_es::proto
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -2,6 +2,11 @@
|
|||||||
#define CMVR_ES_ABSTRACT_CAMERA_H
|
#define CMVR_ES_ABSTRACT_CAMERA_H
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <chrono>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
#include <opencv2/opencv.hpp>
|
#include <opencv2/opencv.hpp>
|
||||||
#include "../abstract_device.h"
|
#include "../abstract_device.h"
|
||||||
#include <Eigen/Core>
|
#include <Eigen/Core>
|
||||||
@ -31,13 +36,47 @@ namespace cmvr::device {
|
|||||||
std::vector<uint8_t> depthFrame;
|
std::vector<uint8_t> depthFrame;
|
||||||
//编码格式
|
//编码格式
|
||||||
std::string codec = ".h264";
|
std::string codec = ".h264";
|
||||||
Rs2Intrinsics intrinsics;
|
Rs2Intrinsics intrinsics{};
|
||||||
int width;
|
int width = 0;
|
||||||
int height;
|
int height = 0;
|
||||||
int fps;
|
int fps = 0;
|
||||||
bool bKey;
|
bool bKey = false;
|
||||||
bool depthKey;
|
bool depthKey = false;
|
||||||
|
|
||||||
|
// Protocol-neutral real-time metadata. The producer fills these values
|
||||||
|
// when a complete encoded access unit is published.
|
||||||
|
uint64_t stream_epoch = 0;
|
||||||
|
uint64_t sequence = 0;
|
||||||
|
// Opaque producer-native timing/counter values. Their units and epoch
|
||||||
|
// are source-defined; zero means that the source did not provide them.
|
||||||
|
uint64_t source_timestamp = 0;
|
||||||
|
uint64_t source_frame_number = 0;
|
||||||
|
int64_t capture_monotonic_ns = 0;
|
||||||
|
int64_t capture_utc_ns = 0;
|
||||||
|
int64_t pts = 0;
|
||||||
|
int64_t dts = 0;
|
||||||
|
int32_t time_base_num = 1;
|
||||||
|
int32_t time_base_den = 1;
|
||||||
|
int64_t duration = 0;
|
||||||
|
bool discontinuity = false;
|
||||||
|
uint32_t codec_config_generation = 0;
|
||||||
|
std::vector<uint8_t> codec_config;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum class PtzCommand {
|
||||||
|
TiltUp,
|
||||||
|
TiltDown,
|
||||||
|
PanLeft,
|
||||||
|
PanRight,
|
||||||
|
UpLeft,
|
||||||
|
UpRight,
|
||||||
|
DownLeft,
|
||||||
|
DownRight,
|
||||||
|
ZoomIn,
|
||||||
|
ZoomOut,
|
||||||
|
PanAuto
|
||||||
|
};
|
||||||
|
|
||||||
class AbstractCamera : public AbstractDevice {
|
class AbstractCamera : public AbstractDevice {
|
||||||
public:
|
public:
|
||||||
// 录制状态
|
// 录制状态
|
||||||
@ -51,7 +90,25 @@ namespace cmvr::device {
|
|||||||
~AbstractCamera() override = default;
|
~AbstractCamera() override = default;
|
||||||
|
|
||||||
DeviceKind kind() const noexcept override { return DeviceKind::Camera; }
|
DeviceKind kind() const noexcept override { return DeviceKind::Camera; }
|
||||||
inline void getState(CameraState &state) {state = state_;}
|
// Every implementation must take the same lock used by its state_
|
||||||
|
// writers; CameraState contains std::string and cannot be snapshotted
|
||||||
|
// safely while another thread mutates it.
|
||||||
|
virtual void getState(CameraState &state) = 0;
|
||||||
|
DeviceHealthSnapshot healthSnapshot() override {
|
||||||
|
CameraState state{};
|
||||||
|
getState(state);
|
||||||
|
|
||||||
|
DeviceHealthSnapshot health;
|
||||||
|
health.error_message = state.error_message;
|
||||||
|
if (state.is_error) {
|
||||||
|
health.state = DeviceHealthState::Fault;
|
||||||
|
} else if (!state.error_message.empty()) {
|
||||||
|
health.state = DeviceHealthState::Degraded;
|
||||||
|
} else if (state.is_initialized) {
|
||||||
|
health.state = DeviceHealthState::Healthy;
|
||||||
|
}
|
||||||
|
return health;
|
||||||
|
}
|
||||||
virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {}
|
virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {}
|
||||||
virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
|
virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
|
||||||
virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
|
virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
|
||||||
@ -60,12 +117,27 @@ namespace cmvr::device {
|
|||||||
virtual void pauseRecording() {}
|
virtual void pauseRecording() {}
|
||||||
virtual void resumeRecording() {}
|
virtual void resumeRecording() {}
|
||||||
virtual void getEncodedFrame(StreamFrameData& frame_data, size_t& index) {}
|
virtual void getEncodedFrame(StreamFrameData& frame_data, size_t& index) {}
|
||||||
|
virtual bool waitEncodedFrame(
|
||||||
|
StreamFrameData& frame_data,
|
||||||
|
size_t& index,
|
||||||
|
std::chrono::milliseconds timeout) {
|
||||||
|
(void)timeout;
|
||||||
|
getEncodedFrame(frame_data, index);
|
||||||
|
return !frame_data.rgbFrame.empty() || !frame_data.depthFrame.empty();
|
||||||
|
}
|
||||||
virtual bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) {
|
virtual bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual bool startStreaming() {return true;}
|
virtual bool startStreaming() {return true;}
|
||||||
virtual void stopStreaming() {}
|
virtual void stopStreaming() {}
|
||||||
|
virtual bool controlPtz(PtzCommand command, bool stop, int speed) {
|
||||||
|
(void)command;
|
||||||
|
(void)stop;
|
||||||
|
(void)speed;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
virtual bool requestKeyFrame() { return false; }
|
||||||
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
|
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
|
||||||
protected:
|
protected:
|
||||||
CameraState state_{};
|
CameraState state_{};
|
||||||
|
|||||||
@ -9,6 +9,7 @@
|
|||||||
#include "common/base/logging/logger.h"
|
#include "common/base/logging/logger.h"
|
||||||
#include "devices/camera/abstract_camera.h"
|
#include "devices/camera/abstract_camera.h"
|
||||||
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
|
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
|
||||||
|
#include "devices/camera/hikvision_camera/include/hikvision_camera.h"
|
||||||
#include "devices/camera/realsense_camera/include/realsense_camera.h"
|
#include "devices/camera/realsense_camera/include/realsense_camera.h"
|
||||||
#include "devices/camera/uvc_camera/include/uvc_camera.h"
|
#include "devices/camera/uvc_camera/include/uvc_camera.h"
|
||||||
|
|
||||||
@ -41,6 +42,10 @@ public:
|
|||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case config::CameraDeviceConfig::kHikvision:
|
||||||
|
return std::make_shared<HikvisionCamera>(
|
||||||
|
backendWithId_(cfg.id(), cfg.hikvision()));
|
||||||
|
|
||||||
case config::CameraDeviceConfig::kMujoco:
|
case config::CameraDeviceConfig::kMujoco:
|
||||||
return std::make_shared<MujocoCamera>(
|
return std::make_shared<MujocoCamera>(
|
||||||
backendWithId_(cfg.id(), cfg.mujoco()));
|
backendWithId_(cfg.id(), cfg.mujoco()));
|
||||||
|
|||||||
122
cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt
Normal file
122
cmvr-es/devices/camera/hikvision_camera/CMakeLists.txt
Normal file
@ -0,0 +1,122 @@
|
|||||||
|
add_library(hikvision_camera SHARED src/hikvision_camera.cpp)
|
||||||
|
|
||||||
|
target_include_directories(hikvision_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
|
set(HIKVISION_SDK_ROOT "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/hikvision_sdk/v6.1.11.5")
|
||||||
|
set(HIKVISION_SDK_LIB_DIR "${HIKVISION_SDK_ROOT}/lib")
|
||||||
|
set(HIKVISION_SDK_COM_DIR "${HIKVISION_SDK_LIB_DIR}/HCNetSDKCom")
|
||||||
|
|
||||||
|
file(GLOB HIKVISION_SDK_LIBS CONFIGURE_DEPENDS
|
||||||
|
"${HIKVISION_SDK_LIB_DIR}/*.so"
|
||||||
|
"${HIKVISION_SDK_LIB_DIR}/*.so.*"
|
||||||
|
)
|
||||||
|
file(GLOB HIKVISION_SDK_COM_LIBS CONFIGURE_DEPENDS
|
||||||
|
"${HIKVISION_SDK_COM_DIR}/*.so"
|
||||||
|
"${HIKVISION_SDK_COM_DIR}/*.so.*"
|
||||||
|
)
|
||||||
|
set(HIKVISION_HCNETSDK_LIB "${HIKVISION_SDK_LIB_DIR}/libhcnetsdk.so")
|
||||||
|
|
||||||
|
if(NOT HIKVISION_SDK_LIBS)
|
||||||
|
message(FATAL_ERROR "Hikvision SDK libraries not found: ${HIKVISION_SDK_LIB_DIR}")
|
||||||
|
endif()
|
||||||
|
if(NOT HIKVISION_SDK_COM_LIBS)
|
||||||
|
message(FATAL_ERROR "Hikvision SDK component libraries not found: ${HIKVISION_SDK_COM_DIR}")
|
||||||
|
endif()
|
||||||
|
if(NOT EXISTS "${HIKVISION_HCNETSDK_LIB}")
|
||||||
|
message(FATAL_ERROR "Hikvision HCNetSDK library not found: ${HIKVISION_HCNETSDK_LIB}")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
target_include_directories(hikvision_camera PRIVATE "${HIKVISION_SDK_ROOT}/include")
|
||||||
|
target_link_directories(hikvision_camera PRIVATE "${HIKVISION_SDK_LIB_DIR}" "${HIKVISION_SDK_COM_DIR}")
|
||||||
|
|
||||||
|
set(HIKVISION_JSONCPP_LINK jsoncpp)
|
||||||
|
if(TARGET jsoncpp)
|
||||||
|
get_target_property(HIKVISION_JSONCPP_IMPORTED_LOCATION jsoncpp IMPORTED_LOCATION)
|
||||||
|
get_target_property(HIKVISION_JSONCPP_IMPORTED_LOCATION_RELEASE jsoncpp IMPORTED_LOCATION_RELEASE)
|
||||||
|
get_target_property(HIKVISION_JSONCPP_INTERFACE_INCLUDES jsoncpp INTERFACE_INCLUDE_DIRECTORIES)
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp target: jsoncpp")
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp IMPORTED_LOCATION: ${HIKVISION_JSONCPP_IMPORTED_LOCATION}")
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp IMPORTED_LOCATION_RELEASE: ${HIKVISION_JSONCPP_IMPORTED_LOCATION_RELEASE}")
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp INTERFACE_INCLUDE_DIRECTORIES: ${HIKVISION_JSONCPP_INTERFACE_INCLUDES}")
|
||||||
|
else()
|
||||||
|
find_library(HIKVISION_JSONCPP_LIBRARY NAMES jsoncpp)
|
||||||
|
if(HIKVISION_JSONCPP_LIBRARY)
|
||||||
|
set(HIKVISION_JSONCPP_LINK "${HIKVISION_JSONCPP_LIBRARY}")
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp library: ${HIKVISION_JSONCPP_LIBRARY}")
|
||||||
|
else()
|
||||||
|
message(WARNING "[hikvision_camera] jsoncpp library not found by CMake; linker will resolve -ljsoncpp")
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
find_path(HIKVISION_JSONCPP_INCLUDE_DIR NAMES json/json.h PATH_SUFFIXES jsoncpp)
|
||||||
|
message(STATUS "[hikvision_camera] jsoncpp include dir: ${HIKVISION_JSONCPP_INCLUDE_DIR}")
|
||||||
|
if(HIKVISION_JSONCPP_INCLUDE_DIR)
|
||||||
|
target_include_directories(hikvision_camera PRIVATE "${HIKVISION_JSONCPP_INCLUDE_DIR}")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
target_link_libraries(hikvision_camera
|
||||||
|
PUBLIC
|
||||||
|
glog
|
||||||
|
opencv_core
|
||||||
|
cmvr_es::proto
|
||||||
|
PRIVATE
|
||||||
|
"${HIKVISION_HCNETSDK_LIB}"
|
||||||
|
${HIKVISION_JSONCPP_LINK}
|
||||||
|
pthread
|
||||||
|
)
|
||||||
|
|
||||||
|
set_target_properties(hikvision_camera PROPERTIES
|
||||||
|
BUILD_RPATH "${HIKVISION_SDK_LIB_DIR};${HIKVISION_SDK_COM_DIR}"
|
||||||
|
INSTALL_RPATH "$ORIGIN;$ORIGIN/HCNetSDKCom"
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(cmvr_es::device::hikvision_camera ALIAS hikvision_camera)
|
||||||
|
install(TARGETS hikvision_camera LIBRARY DESTINATION lib)
|
||||||
|
install(FILES ${HIKVISION_SDK_LIBS} DESTINATION lib)
|
||||||
|
install(DIRECTORY "${HIKVISION_SDK_COM_DIR}" DESTINATION lib)
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
add_executable(hikvision_camera_callback_test
|
||||||
|
tests/hikvision_camera_callback_test.cpp
|
||||||
|
src/hikvision_camera.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(hikvision_camera_callback_test
|
||||||
|
PRIVATE
|
||||||
|
${CMAKE_CURRENT_SOURCE_DIR}
|
||||||
|
"${HIKVISION_SDK_ROOT}/include"
|
||||||
|
)
|
||||||
|
if(HIKVISION_JSONCPP_INCLUDE_DIR)
|
||||||
|
target_include_directories(
|
||||||
|
hikvision_camera_callback_test
|
||||||
|
PRIVATE
|
||||||
|
"${HIKVISION_JSONCPP_INCLUDE_DIR}"
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
target_compile_definitions(hikvision_camera_callback_test
|
||||||
|
PRIVATE
|
||||||
|
HIKVISION_SDK_LIB_DIR="${HIKVISION_SDK_LIB_DIR}/"
|
||||||
|
)
|
||||||
|
# The test supplies a small in-process HCNetSDK fake, so it exercises the
|
||||||
|
# callback/stop ordering without requiring a camera or loading hcnetsdk.
|
||||||
|
target_link_libraries(hikvision_camera_callback_test
|
||||||
|
PRIVATE
|
||||||
|
glog
|
||||||
|
opencv_core
|
||||||
|
cmvr_es::proto
|
||||||
|
${HIKVISION_JSONCPP_LINK}
|
||||||
|
pthread
|
||||||
|
)
|
||||||
|
add_test(
|
||||||
|
NAME hikvision_camera_callback_test
|
||||||
|
COMMAND hikvision_camera_callback_test
|
||||||
|
)
|
||||||
|
set_tests_properties(hikvision_camera_callback_test PROPERTIES TIMEOUT 10)
|
||||||
|
if(UNIX AND NOT APPLE)
|
||||||
|
get_property(_hikvision_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES)
|
||||||
|
list(PREPEND _hikvision_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
|
||||||
|
list(JOIN _hikvision_test_library_dirs ":" _hikvision_test_library_path)
|
||||||
|
set_tests_properties(hikvision_camera_callback_test PROPERTIES
|
||||||
|
ENVIRONMENT "LD_LIBRARY_PATH=${_hikvision_test_library_path}"
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
@ -0,0 +1,117 @@
|
|||||||
|
#ifndef CMVR_ES_HIKVISION_CAMERA_H
|
||||||
|
#define CMVR_ES_HIKVISION_CAMERA_H
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <memory>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/base/ring_buffer.h"
|
||||||
|
#include "devices/camera/abstract_camera.h"
|
||||||
|
|
||||||
|
namespace cmvr::device {
|
||||||
|
|
||||||
|
class HikvisionCamera final : public AbstractCamera {
|
||||||
|
public:
|
||||||
|
explicit HikvisionCamera(const cmvr::config::HikvisionCameraConfig& camera);
|
||||||
|
~HikvisionCamera() override;
|
||||||
|
|
||||||
|
std::string typeName() const override { return "HikvisionCamera"; }
|
||||||
|
bool init() override;
|
||||||
|
bool start() override;
|
||||||
|
bool stop() override;
|
||||||
|
void getState(CameraState& state) override;
|
||||||
|
|
||||||
|
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
||||||
|
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
||||||
|
void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
||||||
|
|
||||||
|
void startRecording(const std::string& video_path) override;
|
||||||
|
void stopRecording() override;
|
||||||
|
void pauseRecording() override;
|
||||||
|
void resumeRecording() override;
|
||||||
|
void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
|
||||||
|
bool waitEncodedFrame(StreamFrameData& frame_data, size_t& index, std::chrono::milliseconds timeout) override;
|
||||||
|
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
|
||||||
|
bool startStreaming() override;
|
||||||
|
void stopStreaming() override;
|
||||||
|
bool controlPtz(PtzCommand command, bool stop, int speed) override;
|
||||||
|
bool executeJsonCommand(const std::string& request_json, std::string& response_json) override;
|
||||||
|
bool requestKeyFrame() override;
|
||||||
|
|
||||||
|
void onEsData(long real_handle,
|
||||||
|
unsigned int packet_type,
|
||||||
|
unsigned char* buffer,
|
||||||
|
unsigned int buffer_size,
|
||||||
|
unsigned int width,
|
||||||
|
unsigned int height,
|
||||||
|
uint64_t source_timestamp,
|
||||||
|
uint64_t source_frame_number,
|
||||||
|
unsigned int source_frame_rate,
|
||||||
|
unsigned int source_packet_mode);
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool initSdk_();
|
||||||
|
void releaseSdk_();
|
||||||
|
bool login_();
|
||||||
|
bool startPreview_();
|
||||||
|
void stopPreview_();
|
||||||
|
bool requestKeyFrame_();
|
||||||
|
void stopRecordingUnlocked_();
|
||||||
|
void fillIntrinsics_(Rs2Intrinsics& intrinsics) const;
|
||||||
|
void setError_(const std::string& message);
|
||||||
|
std::string sdkError_(const std::string& action) const;
|
||||||
|
void pushEncodedFrame_(const unsigned char* buffer,
|
||||||
|
unsigned int buffer_size,
|
||||||
|
bool is_key_frame,
|
||||||
|
unsigned int width,
|
||||||
|
unsigned int height,
|
||||||
|
uint64_t source_timestamp,
|
||||||
|
uint64_t source_frame_number,
|
||||||
|
unsigned int source_frame_rate,
|
||||||
|
unsigned int source_packet_mode);
|
||||||
|
void resetStreamState_();
|
||||||
|
|
||||||
|
config::HikvisionCameraConfig camera_;
|
||||||
|
std::string ip_;
|
||||||
|
std::string username_;
|
||||||
|
std::string password_;
|
||||||
|
std::string sdk_path_;
|
||||||
|
|
||||||
|
int port_ = 8000;
|
||||||
|
int channel_ = 1;
|
||||||
|
int stream_type_ = 0;
|
||||||
|
int link_mode_ = 0;
|
||||||
|
int fps_ = 25;
|
||||||
|
int width_ = 0;
|
||||||
|
int height_ = 0;
|
||||||
|
size_t buffer_size_ = 30;
|
||||||
|
std::string codec_ = "H264";
|
||||||
|
|
||||||
|
int user_id_ = -1;
|
||||||
|
int real_handle_ = -1;
|
||||||
|
bool sdk_acquired_ = false;
|
||||||
|
|
||||||
|
std::shared_ptr<SPMCRingBuffer<StreamFrameData>> stream_frame_buffer_;
|
||||||
|
std::string current_video_path_;
|
||||||
|
|
||||||
|
mutable std::mutex ctrl_mtx_;
|
||||||
|
// SDK callbacks must never take ctrl_mtx_: NET_DVR_StopRealPlay may wait
|
||||||
|
// for an in-flight callback while stop() owns that mutex. This mutex is the
|
||||||
|
// single synchronization domain for callback publication and stream state.
|
||||||
|
mutable std::mutex callback_mtx_;
|
||||||
|
bool callback_publishing_enabled_ = false;
|
||||||
|
long callback_preview_handle_ = -1;
|
||||||
|
std::vector<uint8_t> es_stream_header_;
|
||||||
|
bool has_es_stream_header_ = false;
|
||||||
|
bool awaiting_key_frame_ = false;
|
||||||
|
int stream_count_ = 0;
|
||||||
|
uint64_t stream_epoch_ = 0;
|
||||||
|
uint64_t stream_sequence_ = 0;
|
||||||
|
uint32_t codec_config_generation_ = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::device
|
||||||
|
|
||||||
|
#endif // CMVR_ES_HIKVISION_CAMERA_H
|
||||||
961
cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp
Normal file
961
cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp
Normal file
@ -0,0 +1,961 @@
|
|||||||
|
#include "../include/hikvision_camera.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <atomic>
|
||||||
|
#include <cctype>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <cstdio>
|
||||||
|
#include <cstring>
|
||||||
|
#include <filesystem>
|
||||||
|
#include <limits.h>
|
||||||
|
#include <sstream>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#if defined(__linux__)
|
||||||
|
#include <unistd.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include "HCNetSDK.h"
|
||||||
|
#include "common/base/logging/logger.h"
|
||||||
|
#include "json/json.h"
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
std::mutex g_sdk_mutex;
|
||||||
|
int g_sdk_ref_count = 0;
|
||||||
|
bool g_sdk_initialized = false;
|
||||||
|
std::atomic<int> g_ignored_data_type_log_count{0};
|
||||||
|
std::atomic<int> g_es_video_log_count{0};
|
||||||
|
std::atomic<int> g_unexpected_packet_mode_log_count{0};
|
||||||
|
|
||||||
|
constexpr DWORD kHikvisionPacketFileHeader = 0;
|
||||||
|
constexpr DWORD kHikvisionPacketVideoIFrame = 1;
|
||||||
|
constexpr DWORD kHikvisionPacketVideoBFrame = 2;
|
||||||
|
constexpr DWORD kHikvisionPacketVideoPFrame = 3;
|
||||||
|
|
||||||
|
std::string normalizeSdkPath(std::string path)
|
||||||
|
{
|
||||||
|
if (path.empty()) {
|
||||||
|
return path;
|
||||||
|
}
|
||||||
|
const char last = path.back();
|
||||||
|
if (last != '/' && last != '\\') {
|
||||||
|
path.push_back('/');
|
||||||
|
}
|
||||||
|
return path;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string defaultRuntimeSdkPath()
|
||||||
|
{
|
||||||
|
#if defined(__linux__)
|
||||||
|
char executable_path[PATH_MAX] = {0};
|
||||||
|
const ssize_t count = readlink("/proc/self/exe", executable_path, PATH_MAX - 1);
|
||||||
|
if (count > 0) {
|
||||||
|
executable_path[count] = '\0';
|
||||||
|
const auto lib_path =
|
||||||
|
std::filesystem::path(executable_path).parent_path() / ".." / "lib";
|
||||||
|
return normalizeSdkPath(std::filesystem::weakly_canonical(lib_path).string());
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
return normalizeSdkPath((std::filesystem::current_path() / ".." / "lib").string());
|
||||||
|
}
|
||||||
|
|
||||||
|
void copyCString(char* dest, size_t dest_size, const std::string& source)
|
||||||
|
{
|
||||||
|
if (dest_size == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
std::snprintf(dest, dest_size, "%s", source.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
DWORD toHikvisionPtzCommand(cmvr::device::PtzCommand command)
|
||||||
|
{
|
||||||
|
switch (command) {
|
||||||
|
case cmvr::device::PtzCommand::TiltUp:
|
||||||
|
return TILT_UP;
|
||||||
|
case cmvr::device::PtzCommand::TiltDown:
|
||||||
|
return TILT_DOWN;
|
||||||
|
case cmvr::device::PtzCommand::PanLeft:
|
||||||
|
return PAN_LEFT;
|
||||||
|
case cmvr::device::PtzCommand::PanRight:
|
||||||
|
return PAN_RIGHT;
|
||||||
|
case cmvr::device::PtzCommand::UpLeft:
|
||||||
|
return UP_LEFT;
|
||||||
|
case cmvr::device::PtzCommand::UpRight:
|
||||||
|
return UP_RIGHT;
|
||||||
|
case cmvr::device::PtzCommand::DownLeft:
|
||||||
|
return DOWN_LEFT;
|
||||||
|
case cmvr::device::PtzCommand::DownRight:
|
||||||
|
return DOWN_RIGHT;
|
||||||
|
case cmvr::device::PtzCommand::ZoomIn:
|
||||||
|
return ZOOM_IN;
|
||||||
|
case cmvr::device::PtzCommand::ZoomOut:
|
||||||
|
return ZOOM_OUT;
|
||||||
|
case cmvr::device::PtzCommand::PanAuto:
|
||||||
|
return PAN_AUTO;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
DWORD normalizePtzSpeed(int speed)
|
||||||
|
{
|
||||||
|
if (speed <= 0) {
|
||||||
|
return 4;
|
||||||
|
}
|
||||||
|
if (speed > 7) {
|
||||||
|
return 7;
|
||||||
|
}
|
||||||
|
return static_cast<DWORD>(speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string lowerString(std::string value)
|
||||||
|
{
|
||||||
|
std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) {
|
||||||
|
return static_cast<char>(std::tolower(c));
|
||||||
|
});
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string jsonEscape(const std::string& value)
|
||||||
|
{
|
||||||
|
std::ostringstream out;
|
||||||
|
for (const char c : value) {
|
||||||
|
switch (c) {
|
||||||
|
case '\\':
|
||||||
|
out << "\\\\";
|
||||||
|
break;
|
||||||
|
case '"':
|
||||||
|
out << "\\\"";
|
||||||
|
break;
|
||||||
|
case '\n':
|
||||||
|
out << "\\n";
|
||||||
|
break;
|
||||||
|
case '\r':
|
||||||
|
out << "\\r";
|
||||||
|
break;
|
||||||
|
case '\t':
|
||||||
|
out << "\\t";
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
out << c;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return out.str();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string jsonStringField(const Json::Value& value, const char* name)
|
||||||
|
{
|
||||||
|
const Json::Value* member = value.find(name, name + std::strlen(name));
|
||||||
|
return member && member->isString() ? member->asString() : "";
|
||||||
|
}
|
||||||
|
|
||||||
|
bool jsonBoolField(const Json::Value& value, const char* name, bool& out)
|
||||||
|
{
|
||||||
|
const Json::Value* member = value.find(name, name + std::strlen(name));
|
||||||
|
if (!member || !member->isBool()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
out = member->asBool();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
int jsonIntField(const Json::Value& value, const char* name, int default_value)
|
||||||
|
{
|
||||||
|
const Json::Value* member = value.find(name, name + std::strlen(name));
|
||||||
|
return member && member->isInt() ? member->asInt() : default_value;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool parseJsonCommand(const std::string& request_json, Json::Value& root, std::string& error)
|
||||||
|
{
|
||||||
|
Json::CharReaderBuilder builder;
|
||||||
|
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
|
||||||
|
return reader->parse(
|
||||||
|
request_json.data(), request_json.data() + request_json.size(), &root, &error);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool parsePtzCommandName(const std::string& command, cmvr::device::PtzCommand& out)
|
||||||
|
{
|
||||||
|
const std::string normalized = lowerString(command);
|
||||||
|
if (normalized == "tilt_up" || normalized == "up") {
|
||||||
|
out = cmvr::device::PtzCommand::TiltUp;
|
||||||
|
} else if (normalized == "tilt_down" || normalized == "down") {
|
||||||
|
out = cmvr::device::PtzCommand::TiltDown;
|
||||||
|
} else if (normalized == "pan_left" || normalized == "left") {
|
||||||
|
out = cmvr::device::PtzCommand::PanLeft;
|
||||||
|
} else if (normalized == "pan_right" || normalized == "right") {
|
||||||
|
out = cmvr::device::PtzCommand::PanRight;
|
||||||
|
} else if (normalized == "up_left") {
|
||||||
|
out = cmvr::device::PtzCommand::UpLeft;
|
||||||
|
} else if (normalized == "up_right") {
|
||||||
|
out = cmvr::device::PtzCommand::UpRight;
|
||||||
|
} else if (normalized == "down_left") {
|
||||||
|
out = cmvr::device::PtzCommand::DownLeft;
|
||||||
|
} else if (normalized == "down_right") {
|
||||||
|
out = cmvr::device::PtzCommand::DownRight;
|
||||||
|
} else if (normalized == "zoom_in") {
|
||||||
|
out = cmvr::device::PtzCommand::ZoomIn;
|
||||||
|
} else if (normalized == "zoom_out") {
|
||||||
|
out = cmvr::device::PtzCommand::ZoomOut;
|
||||||
|
} else if (normalized == "pan_auto" || normalized == "auto") {
|
||||||
|
out = cmvr::device::PtzCommand::PanAuto;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string makeJsonResult(bool success, const std::string& error_message = "")
|
||||||
|
{
|
||||||
|
std::ostringstream out;
|
||||||
|
out << "{\"success\":" << (success ? "true" : "false");
|
||||||
|
if (!error_message.empty()) {
|
||||||
|
out << ",\"error_message\":\"" << jsonEscape(error_message) << "\"";
|
||||||
|
}
|
||||||
|
out << "}";
|
||||||
|
return out.str();
|
||||||
|
}
|
||||||
|
|
||||||
|
void CALLBACK hikvisionEsRealPlayCallback(
|
||||||
|
LONG real_handle, NET_DVR_PACKET_INFO_EX* packet_info, void* user)
|
||||||
|
{
|
||||||
|
auto* camera = static_cast<cmvr::device::HikvisionCamera*>(user);
|
||||||
|
if (!camera || !packet_info) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
const uint64_t source_timestamp =
|
||||||
|
(static_cast<uint64_t>(packet_info->dwTimeStampHigh) << 32U) |
|
||||||
|
static_cast<uint64_t>(packet_info->dwTimeStamp);
|
||||||
|
camera->onEsData(real_handle,
|
||||||
|
packet_info->dwPacketType,
|
||||||
|
packet_info->pPacketBuffer,
|
||||||
|
packet_info->dwPacketSize,
|
||||||
|
packet_info->wWidth,
|
||||||
|
packet_info->wHeight,
|
||||||
|
source_timestamp,
|
||||||
|
packet_info->dwFrameNum,
|
||||||
|
packet_info->dwFrameRate,
|
||||||
|
packet_info->dwPacketMode);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
namespace cmvr::device {
|
||||||
|
|
||||||
|
HikvisionCamera::HikvisionCamera(const config::HikvisionCameraConfig& camera)
|
||||||
|
: camera_(camera)
|
||||||
|
{
|
||||||
|
id_ = camera_.id();
|
||||||
|
ip_ = camera_.ip();
|
||||||
|
username_ = camera_.username().empty() ? "admin" : camera_.username();
|
||||||
|
password_ = camera_.password();
|
||||||
|
port_ = camera_.port() > 0 ? camera_.port() : 8000;
|
||||||
|
channel_ = camera_.channel() > 0 ? camera_.channel() : 1;
|
||||||
|
stream_type_ = camera_.stream_type() >= 0 ? camera_.stream_type() : 0;
|
||||||
|
link_mode_ = camera_.link_mode() >= 0 ? camera_.link_mode() : 0;
|
||||||
|
fps_ = camera_.fps() > 0 ? camera_.fps() : 25;
|
||||||
|
width_ = camera_.width();
|
||||||
|
height_ = camera_.height();
|
||||||
|
buffer_size_ = camera_.buffer_size() > 0 ? static_cast<size_t>(camera_.buffer_size()) : 30;
|
||||||
|
codec_ = camera_.codec().empty() ? "H264" : camera_.codec();
|
||||||
|
sdk_path_ = normalizeSdkPath(
|
||||||
|
camera_.sdk_path().empty() ? defaultRuntimeSdkPath() : camera_.sdk_path());
|
||||||
|
// Keep the shared_ptr itself immutable after construction. Readers and the
|
||||||
|
// SDK callback may use it concurrently; the ring buffer owns its locking.
|
||||||
|
stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_);
|
||||||
|
|
||||||
|
state_.fps = fps_;
|
||||||
|
state_.width = width_;
|
||||||
|
state_.height = height_;
|
||||||
|
|
||||||
|
if (id_.empty()) {
|
||||||
|
setError_("[HikvisionCamera] camera id is empty");
|
||||||
|
} else if (ip_.empty()) {
|
||||||
|
setError_("[HikvisionCamera] ip is empty");
|
||||||
|
} else if (password_.empty()) {
|
||||||
|
setError_("[HikvisionCamera] password is empty");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
HikvisionCamera::~HikvisionCamera()
|
||||||
|
{
|
||||||
|
stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::init()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
state_.is_initialized = false;
|
||||||
|
|
||||||
|
if (ip_.empty() || username_.empty() || password_.empty()) {
|
||||||
|
setError_("ip, username or password is empty");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
stream_frame_buffer_->clear();
|
||||||
|
resetStreamState_();
|
||||||
|
|
||||||
|
if (!initSdk_()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
state_.is_initialized = true;
|
||||||
|
state_.is_error = false;
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] initialized: id=" << id_
|
||||||
|
<< ", ip=" << ip_
|
||||||
|
<< ", channel=" << channel_
|
||||||
|
<< ", stream_type=" << stream_type_;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::start()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
|
||||||
|
if (!state_.is_initialized) {
|
||||||
|
setError_("camera not initialized");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (state_.is_opened) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (!login_()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!startPreview_()) {
|
||||||
|
if (user_id_ >= 0) {
|
||||||
|
NET_DVR_Logout(user_id_);
|
||||||
|
user_id_ = -1;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
state_.is_opened = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::stop()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
if (state_.is_recording) {
|
||||||
|
stopRecordingUnlocked_();
|
||||||
|
}
|
||||||
|
state_.is_streaming = false;
|
||||||
|
stream_count_ = 0;
|
||||||
|
resetStreamState_();
|
||||||
|
stopPreview_();
|
||||||
|
if (user_id_ >= 0) {
|
||||||
|
NET_DVR_Logout(user_id_);
|
||||||
|
user_id_ = -1;
|
||||||
|
}
|
||||||
|
state_.is_opened = false;
|
||||||
|
releaseSdk_();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::getState(CameraState& state)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
state = state_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
fillIntrinsics_(intrinsics);
|
||||||
|
setError_("getRGBImage unsupported: HikvisionCamera only forwards stream data");
|
||||||
|
color.release();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics&)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
setError_("getDepthImage unsupported usage");
|
||||||
|
depth.release();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
|
||||||
|
{
|
||||||
|
getRGBImage(color, intrinsics);
|
||||||
|
depth.release();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::startRecording(const std::string& video_path)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
|
||||||
|
if (!state_.is_opened || real_handle_ < 0) {
|
||||||
|
setError_("camera not opened");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (state_.is_recording) {
|
||||||
|
setError_("already recording");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (video_path.empty()) {
|
||||||
|
setError_("video path is empty");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
current_video_path_ = video_path;
|
||||||
|
if (!NET_DVR_SaveRealData(real_handle_, const_cast<char*>(current_video_path_.c_str()))) {
|
||||||
|
setError_(sdkError_("NET_DVR_SaveRealData"));
|
||||||
|
current_video_path_.clear();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
state_.is_recording = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::stopRecording()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
stopRecordingUnlocked_();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::stopRecordingUnlocked_()
|
||||||
|
{
|
||||||
|
clear_error_();
|
||||||
|
|
||||||
|
if (!state_.is_recording) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (real_handle_ >= 0 && !NET_DVR_StopSaveRealData(real_handle_)) {
|
||||||
|
setError_(sdkError_("NET_DVR_StopSaveRealData"));
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
state_.is_recording = false;
|
||||||
|
current_video_path_.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::pauseRecording()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
setError_("pauseRecording unsupported by Hikvision SDK recording");
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::resumeRecording()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
setError_("resumeRecording unsupported by Hikvision SDK recording");
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index)
|
||||||
|
{
|
||||||
|
if (!stream_frame_buffer_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
auto frame = stream_frame_buffer_->pop(index);
|
||||||
|
if (frame) {
|
||||||
|
frame_data = std::move(*frame);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::waitEncodedFrame(
|
||||||
|
StreamFrameData& frame_data,
|
||||||
|
size_t& index,
|
||||||
|
const std::chrono::milliseconds timeout)
|
||||||
|
{
|
||||||
|
if (!stream_frame_buffer_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
auto frame = stream_frame_buffer_->waitPop(index, timeout);
|
||||||
|
if (!frame) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
frame_data = std::move(*frame);
|
||||||
|
return !frame_data.rgbFrame.empty() || !frame_data.depthFrame.empty();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index)
|
||||||
|
{
|
||||||
|
if (!stream_frame_buffer_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto frame = stream_frame_buffer_->getLatest(next_index);
|
||||||
|
if (!frame.has_value()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
frame_data = frame.value();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::startStreaming()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
|
||||||
|
if (!state_.is_opened) {
|
||||||
|
setError_("camera not opened");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool first_stream = stream_count_ == 0;
|
||||||
|
if (first_stream) {
|
||||||
|
if (stream_frame_buffer_) {
|
||||||
|
stream_frame_buffer_->clear();
|
||||||
|
}
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
if (callback_preview_handle_ < 0 ||
|
||||||
|
callback_preview_handle_ != static_cast<long>(real_handle_)) {
|
||||||
|
setError_("preview callback is not active");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
++stream_epoch_;
|
||||||
|
stream_sequence_ = 0;
|
||||||
|
++codec_config_generation_;
|
||||||
|
awaiting_key_frame_ = true;
|
||||||
|
callback_publishing_enabled_ = true;
|
||||||
|
}
|
||||||
|
state_.is_streaming = true;
|
||||||
|
++stream_count_;
|
||||||
|
if (first_stream && !requestKeyFrame_()) {
|
||||||
|
// Keep waiting for the next natural I-frame. Publishing P/B frames
|
||||||
|
// immediately would make a newly attached decoder start corrupted.
|
||||||
|
CMVR_LOG(WARNING) << "[HikvisionCamera] key-frame request failed; "
|
||||||
|
<< "waiting for the next natural I-frame";
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::stopStreaming()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
if (stream_count_ > 0) {
|
||||||
|
--stream_count_;
|
||||||
|
}
|
||||||
|
if (stream_count_ == 0) {
|
||||||
|
// Wait for an already-running callback to finish its publication, then
|
||||||
|
// prevent both queued and future callbacks from publishing. No frame
|
||||||
|
// can be pushed after this critical section has completed.
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
callback_publishing_enabled_ = false;
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
state_.is_streaming = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
|
||||||
|
if (!state_.is_opened || user_id_ < 0) {
|
||||||
|
setError_("camera not opened");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const DWORD sdk_command = toHikvisionPtzCommand(command);
|
||||||
|
if (sdk_command == 0) {
|
||||||
|
setError_("unsupported PTZ command");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const DWORD sdk_stop = stop ? 1 : 0;
|
||||||
|
const DWORD sdk_speed = normalizePtzSpeed(speed);
|
||||||
|
if (!NET_DVR_PTZControlWithSpeed_Other(user_id_, channel_, sdk_command, sdk_stop, sdk_speed)) {
|
||||||
|
setError_(sdkError_("NET_DVR_PTZControlWithSpeed_Other"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::requestKeyFrame()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
clear_error_();
|
||||||
|
if (!state_.is_opened || user_id_ < 0) {
|
||||||
|
setError_("camera not opened");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!requestKeyFrame_()) {
|
||||||
|
setError_(sdkError_(
|
||||||
|
stream_type_ == 0 ? "NET_DVR_MakeKeyFrame" : "NET_DVR_MakeKeyFrameSub"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::requestKeyFrame_()
|
||||||
|
{
|
||||||
|
if (user_id_ < 0) {
|
||||||
|
CMVR_LOG(WARNING) << "[HikvisionCamera] skip key frame request: camera not logged in";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool is_sub_stream = stream_type_ != 0;
|
||||||
|
const BOOL success = is_sub_stream
|
||||||
|
? NET_DVR_MakeKeyFrameSub(user_id_, channel_)
|
||||||
|
: NET_DVR_MakeKeyFrame(user_id_, channel_);
|
||||||
|
if (!success) {
|
||||||
|
CMVR_LOG(WARNING) << "[HikvisionCamera] "
|
||||||
|
<< (is_sub_stream ? "NET_DVR_MakeKeyFrameSub" : "NET_DVR_MakeKeyFrame")
|
||||||
|
<< " failed, error_code=" << NET_DVR_GetLastError();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] requested key frame"
|
||||||
|
<< ", channel=" << channel_
|
||||||
|
<< ", stream_type=" << stream_type_;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::executeJsonCommand(const std::string& request_json, std::string& response_json)
|
||||||
|
{
|
||||||
|
Json::Value root;
|
||||||
|
std::string parse_error;
|
||||||
|
if (!parseJsonCommand(request_json, root, parse_error) || !root.isObject()) {
|
||||||
|
response_json = makeJsonResult(false, "invalid json: " + parse_error);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string command_type = lowerString(
|
||||||
|
!jsonStringField(root, "command").empty() ? jsonStringField(root, "command") :
|
||||||
|
!jsonStringField(root, "type").empty() ? jsonStringField(root, "type") :
|
||||||
|
jsonStringField(root, "action"));
|
||||||
|
if (command_type != "ptz") {
|
||||||
|
response_json = makeJsonResult(false, "unsupported json command: " + command_type);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string ptz_command_name =
|
||||||
|
!jsonStringField(root, "direction").empty() ? jsonStringField(root, "direction") :
|
||||||
|
!jsonStringField(root, "ptz_command").empty() ? jsonStringField(root, "ptz_command") :
|
||||||
|
jsonStringField(root, "operation");
|
||||||
|
|
||||||
|
PtzCommand ptz_command{};
|
||||||
|
if (!parsePtzCommandName(ptz_command_name, ptz_command)) {
|
||||||
|
response_json = makeJsonResult(false, "invalid ptz command: " + ptz_command_name);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool stop = false;
|
||||||
|
if (!jsonBoolField(root, "stop", stop)) {
|
||||||
|
const std::string ptz_action = lowerString(jsonStringField(root, "ptz_action"));
|
||||||
|
const std::string action = lowerString(jsonStringField(root, "action"));
|
||||||
|
stop = ptz_action == "stop" || action == "stop";
|
||||||
|
}
|
||||||
|
|
||||||
|
const int speed = jsonIntField(root, "speed", 0);
|
||||||
|
if (!controlPtz(ptz_command, stop, speed)) {
|
||||||
|
CameraState state;
|
||||||
|
getState(state);
|
||||||
|
response_json = makeJsonResult(
|
||||||
|
false,
|
||||||
|
state.error_message.empty() ? "failed to control PTZ" : state.error_message);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::ostringstream result;
|
||||||
|
result << "{\"success\":true"
|
||||||
|
<< ",\"command\":\"ptz\""
|
||||||
|
<< ",\"direction\":\"" << jsonEscape(ptz_command_name) << "\""
|
||||||
|
<< ",\"stop\":" << (stop ? "true" : "false")
|
||||||
|
<< ",\"speed\":" << static_cast<int>(normalizePtzSpeed(speed))
|
||||||
|
<< "}";
|
||||||
|
response_json = result.str();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::onEsData(
|
||||||
|
const long real_handle,
|
||||||
|
const unsigned int packet_type,
|
||||||
|
unsigned char* buffer,
|
||||||
|
const unsigned int buffer_size,
|
||||||
|
const unsigned int packet_width,
|
||||||
|
const unsigned int packet_height,
|
||||||
|
const uint64_t source_timestamp,
|
||||||
|
const uint64_t source_frame_number,
|
||||||
|
const unsigned int source_frame_rate,
|
||||||
|
const unsigned int source_packet_mode)
|
||||||
|
{
|
||||||
|
if (!buffer || buffer_size == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Never take ctrl_mtx_ from an SDK callback. NET_DVR_StopRealPlay may wait
|
||||||
|
// for this callback while stop() owns ctrl_mtx_. Holding callback_mtx_
|
||||||
|
// through publication also preserves the ring's single-producer contract.
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
if (callback_preview_handle_ != real_handle || !stream_frame_buffer_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (packet_type == kHikvisionPacketFileHeader) {
|
||||||
|
const bool changed =
|
||||||
|
!has_es_stream_header_ ||
|
||||||
|
es_stream_header_.size() != buffer_size ||
|
||||||
|
!std::equal(es_stream_header_.begin(), es_stream_header_.end(), buffer);
|
||||||
|
if (changed) {
|
||||||
|
es_stream_header_.assign(buffer, buffer + buffer_size);
|
||||||
|
has_es_stream_header_ = true;
|
||||||
|
++codec_config_generation_;
|
||||||
|
if (callback_publishing_enabled_) {
|
||||||
|
awaiting_key_frame_ = true;
|
||||||
|
}
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] received ES stream header, size=" << buffer_size;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool is_video_packet =
|
||||||
|
packet_type == kHikvisionPacketVideoIFrame ||
|
||||||
|
packet_type == kHikvisionPacketVideoBFrame ||
|
||||||
|
packet_type == kHikvisionPacketVideoPFrame;
|
||||||
|
if (!is_video_packet) {
|
||||||
|
const int log_count = g_ignored_data_type_log_count.fetch_add(1);
|
||||||
|
if (log_count < 10) {
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] ignore ES packet_type="
|
||||||
|
<< packet_type << ", size=" << buffer_size;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const int video_log_count = g_es_video_log_count.fetch_add(1);
|
||||||
|
if (video_log_count < 10) {
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] ES video packet_type="
|
||||||
|
<< packet_type << ", size=" << buffer_size
|
||||||
|
<< ", source_timestamp=" << source_timestamp
|
||||||
|
<< ", source_frame_number=" << source_frame_number
|
||||||
|
<< ", source_frame_rate=" << source_frame_rate
|
||||||
|
<< ", source_packet_mode=" << source_packet_mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!callback_publishing_enabled_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool is_key_frame = packet_type == kHikvisionPacketVideoIFrame;
|
||||||
|
if (awaiting_key_frame_) {
|
||||||
|
if (!is_key_frame) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
CMVR_LOG(INFO) << "[HikvisionCamera] received first key frame after stream start";
|
||||||
|
}
|
||||||
|
|
||||||
|
pushEncodedFrame_(buffer,
|
||||||
|
buffer_size,
|
||||||
|
is_key_frame,
|
||||||
|
packet_width,
|
||||||
|
packet_height,
|
||||||
|
source_timestamp,
|
||||||
|
source_frame_number,
|
||||||
|
source_frame_rate,
|
||||||
|
source_packet_mode);
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::pushEncodedFrame_(
|
||||||
|
const unsigned char* buffer,
|
||||||
|
unsigned int buffer_size,
|
||||||
|
bool is_key_frame,
|
||||||
|
unsigned int packet_width,
|
||||||
|
unsigned int packet_height,
|
||||||
|
uint64_t source_timestamp,
|
||||||
|
uint64_t source_frame_number,
|
||||||
|
unsigned int source_frame_rate,
|
||||||
|
unsigned int source_packet_mode)
|
||||||
|
{
|
||||||
|
if (!buffer || buffer_size == 0 || !stream_frame_buffer_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
StreamFrameData frame_data;
|
||||||
|
const auto capture_monotonic = std::chrono::steady_clock::now();
|
||||||
|
const auto capture_utc = std::chrono::system_clock::now();
|
||||||
|
frame_data.rgbFrame.assign(buffer, buffer + buffer_size);
|
||||||
|
frame_data.codec = codec_;
|
||||||
|
frame_data.fps =
|
||||||
|
source_frame_rate >= 1U && source_frame_rate <= 1000U
|
||||||
|
? static_cast<int>(source_frame_rate)
|
||||||
|
: fps_;
|
||||||
|
frame_data.width = packet_width > 0 ? static_cast<int>(packet_width) : width_;
|
||||||
|
frame_data.height = packet_height > 0 ? static_cast<int>(packet_height) : height_;
|
||||||
|
frame_data.bKey = is_key_frame;
|
||||||
|
frame_data.stream_epoch = stream_epoch_;
|
||||||
|
frame_data.sequence = stream_sequence_++;
|
||||||
|
frame_data.source_timestamp = source_timestamp;
|
||||||
|
frame_data.source_frame_number = source_frame_number;
|
||||||
|
frame_data.capture_monotonic_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
capture_monotonic.time_since_epoch()).count();
|
||||||
|
frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
capture_utc.time_since_epoch()).count();
|
||||||
|
frame_data.pts = static_cast<int64_t>(frame_data.sequence);
|
||||||
|
frame_data.dts = frame_data.pts;
|
||||||
|
frame_data.time_base_num = 1;
|
||||||
|
frame_data.time_base_den = std::max(1, frame_data.fps);
|
||||||
|
frame_data.duration = 1;
|
||||||
|
frame_data.codec_config_generation = codec_config_generation_;
|
||||||
|
// Hikvision labels packet type 0 as a "file header", but the bundled SDK
|
||||||
|
// does not guarantee that it is a decoder-ready VPS/SPS/PPS blob. Keep it
|
||||||
|
// only for change detection until its format is verified on real hardware.
|
||||||
|
fillIntrinsics_(frame_data.intrinsics);
|
||||||
|
if (source_packet_mode > 1U) {
|
||||||
|
const int log_count = g_unexpected_packet_mode_log_count.fetch_add(1);
|
||||||
|
if (log_count < 5) {
|
||||||
|
CMVR_LOG(WARNING) << "[HikvisionCamera] unexpected ES source_packet_mode="
|
||||||
|
<< source_packet_mode
|
||||||
|
<< ", source_frame_number=" << source_frame_number;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
stream_frame_buffer_->push(frame_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::resetStreamState_()
|
||||||
|
{
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
callback_publishing_enabled_ = false;
|
||||||
|
callback_preview_handle_ = -1;
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
es_stream_header_.clear();
|
||||||
|
has_es_stream_header_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::initSdk_()
|
||||||
|
{
|
||||||
|
if (sdk_acquired_) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(g_sdk_mutex);
|
||||||
|
if (g_sdk_ref_count == 0) {
|
||||||
|
if (!NET_DVR_Init()) {
|
||||||
|
setError_(sdkError_("NET_DVR_Init"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
NET_DVR_SetConnectTime(3000, 3);
|
||||||
|
NET_DVR_SetReconnect(10000, TRUE);
|
||||||
|
NET_DVR_SetLogToFile(3, nullptr, TRUE);
|
||||||
|
g_sdk_initialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
++g_sdk_ref_count;
|
||||||
|
sdk_acquired_ = true;
|
||||||
|
return g_sdk_initialized;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::releaseSdk_()
|
||||||
|
{
|
||||||
|
if (!sdk_acquired_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(g_sdk_mutex);
|
||||||
|
sdk_acquired_ = false;
|
||||||
|
if (g_sdk_ref_count > 0) {
|
||||||
|
--g_sdk_ref_count;
|
||||||
|
}
|
||||||
|
if (g_sdk_ref_count == 0 && g_sdk_initialized) {
|
||||||
|
NET_DVR_Cleanup();
|
||||||
|
g_sdk_initialized = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::login_()
|
||||||
|
{
|
||||||
|
NET_DVR_USER_LOGIN_INFO login_info{};
|
||||||
|
NET_DVR_DEVICEINFO_V40 device_info{};
|
||||||
|
|
||||||
|
copyCString(login_info.sDeviceAddress, sizeof(login_info.sDeviceAddress), ip_);
|
||||||
|
copyCString(login_info.sUserName, sizeof(login_info.sUserName), username_);
|
||||||
|
copyCString(login_info.sPassword, sizeof(login_info.sPassword), password_);
|
||||||
|
login_info.wPort = static_cast<WORD>(port_);
|
||||||
|
login_info.bUseAsynLogin = FALSE;
|
||||||
|
|
||||||
|
user_id_ = NET_DVR_Login_V40(&login_info, &device_info);
|
||||||
|
if (user_id_ < 0) {
|
||||||
|
setError_(sdkError_("NET_DVR_Login_V40"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HikvisionCamera::startPreview_()
|
||||||
|
{
|
||||||
|
NET_DVR_PREVIEWINFO preview_info{};
|
||||||
|
preview_info.lChannel = channel_;
|
||||||
|
preview_info.dwStreamType = static_cast<DWORD>(stream_type_);
|
||||||
|
preview_info.dwLinkMode = static_cast<DWORD>(link_mode_);
|
||||||
|
preview_info.hPlayWnd = 0;
|
||||||
|
preview_info.bBlocked = 1;
|
||||||
|
preview_info.dwDisplayBufNum = 1;
|
||||||
|
preview_info.byReconnect = 1;
|
||||||
|
|
||||||
|
real_handle_ = NET_DVR_RealPlay_V40(user_id_, &preview_info, nullptr, nullptr);
|
||||||
|
if (real_handle_ < 0) {
|
||||||
|
setError_(sdkError_("NET_DVR_RealPlay_V40"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
{
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
callback_publishing_enabled_ = false;
|
||||||
|
callback_preview_handle_ = static_cast<long>(real_handle_);
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
}
|
||||||
|
if (!NET_DVR_SetESRealPlayCallBack(real_handle_, hikvisionEsRealPlayCallback, this)) {
|
||||||
|
setError_(sdkError_("NET_DVR_SetESRealPlayCallBack"));
|
||||||
|
{
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
callback_publishing_enabled_ = false;
|
||||||
|
callback_preview_handle_ = -1;
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
}
|
||||||
|
NET_DVR_StopRealPlay(real_handle_);
|
||||||
|
real_handle_ = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::stopPreview_()
|
||||||
|
{
|
||||||
|
const int preview_handle = real_handle_;
|
||||||
|
{
|
||||||
|
// Invalidate before asking the SDK to stop. We intentionally release
|
||||||
|
// callback_mtx_ before NET_DVR_StopRealPlay because that function may
|
||||||
|
// wait for an SDK callback to return.
|
||||||
|
std::lock_guard callback_lock(callback_mtx_);
|
||||||
|
callback_publishing_enabled_ = false;
|
||||||
|
callback_preview_handle_ = -1;
|
||||||
|
awaiting_key_frame_ = false;
|
||||||
|
}
|
||||||
|
if (preview_handle >= 0) {
|
||||||
|
NET_DVR_StopRealPlay(preview_handle);
|
||||||
|
real_handle_ = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const
|
||||||
|
{
|
||||||
|
intrinsics.fx = camera_.fx();
|
||||||
|
intrinsics.fy = camera_.fy();
|
||||||
|
intrinsics.cx = camera_.cx() > 0.0f ? camera_.cx() : static_cast<float>(width_) * 0.5f;
|
||||||
|
intrinsics.cy = camera_.cy() > 0.0f ? camera_.cy() : static_cast<float>(height_) * 0.5f;
|
||||||
|
for (int i = 0; i < 5; ++i) {
|
||||||
|
intrinsics.coeffs[i] = i < camera_.coeffs_size() ? camera_.coeffs(i) : 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HikvisionCamera::setError_(const std::string& message)
|
||||||
|
{
|
||||||
|
state_.is_error = true;
|
||||||
|
state_.error_message = message;
|
||||||
|
CMVR_LOG(ERROR) << "[HikvisionCamera] " << message;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string HikvisionCamera::sdkError_(const std::string& action) const
|
||||||
|
{
|
||||||
|
return action + " failed, error_code=" + std::to_string(NET_DVR_GetLastError());
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::device
|
||||||
@ -0,0 +1,425 @@
|
|||||||
|
#include "../include/hikvision_camera.h"
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <iostream>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "HCNetSDK.h"
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
using EsDataCallback =
|
||||||
|
void(CALLBACK*)(LONG, NET_DVR_PACKET_INFO_EX*, void*);
|
||||||
|
|
||||||
|
constexpr DWORD kFileHeader = 0;
|
||||||
|
constexpr DWORD kVideoIFrame = 1;
|
||||||
|
constexpr DWORD kVideoPFrame = 3;
|
||||||
|
|
||||||
|
std::mutex g_fake_sdk_mutex;
|
||||||
|
EsDataCallback g_es_data_callback = nullptr;
|
||||||
|
void* g_es_data_user = nullptr;
|
||||||
|
LONG g_real_handle = 42;
|
||||||
|
std::atomic<int> g_stop_callback_count{0};
|
||||||
|
std::atomic<int> g_key_frame_request_count{0};
|
||||||
|
std::atomic<int> g_ptz_call_count{0};
|
||||||
|
std::atomic<DWORD> g_last_ptz_command{0};
|
||||||
|
std::atomic<DWORD> g_last_ptz_stop{0};
|
||||||
|
std::atomic<DWORD> g_last_ptz_speed{0};
|
||||||
|
|
||||||
|
void resetFakeSdk()
|
||||||
|
{
|
||||||
|
std::lock_guard lock(g_fake_sdk_mutex);
|
||||||
|
g_es_data_callback = nullptr;
|
||||||
|
g_es_data_user = nullptr;
|
||||||
|
g_stop_callback_count = 0;
|
||||||
|
g_key_frame_request_count = 0;
|
||||||
|
g_ptz_call_count = 0;
|
||||||
|
g_last_ptz_command = 0;
|
||||||
|
g_last_ptz_stop = 0;
|
||||||
|
g_last_ptz_speed = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void emitEsPacket(
|
||||||
|
const DWORD packet_type,
|
||||||
|
const LONG real_handle,
|
||||||
|
std::vector<BYTE> payload,
|
||||||
|
const WORD width = 640,
|
||||||
|
const WORD height = 360,
|
||||||
|
const DWORD timestamp_low = 0,
|
||||||
|
const DWORD timestamp_high = 0,
|
||||||
|
const DWORD frame_number = 0,
|
||||||
|
const DWORD frame_rate = 0,
|
||||||
|
const DWORD packet_mode = 0)
|
||||||
|
{
|
||||||
|
EsDataCallback callback = nullptr;
|
||||||
|
void* user = nullptr;
|
||||||
|
{
|
||||||
|
std::lock_guard lock(g_fake_sdk_mutex);
|
||||||
|
callback = g_es_data_callback;
|
||||||
|
user = g_es_data_user;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!callback) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
NET_DVR_PACKET_INFO_EX packet{};
|
||||||
|
packet.wWidth = width;
|
||||||
|
packet.wHeight = height;
|
||||||
|
packet.dwTimeStamp = timestamp_low;
|
||||||
|
packet.dwTimeStampHigh = timestamp_high;
|
||||||
|
packet.dwFrameNum = frame_number;
|
||||||
|
packet.dwFrameRate = frame_rate;
|
||||||
|
packet.dwPacketType = packet_type;
|
||||||
|
packet.dwPacketSize = static_cast<DWORD>(payload.size());
|
||||||
|
packet.pPacketBuffer = payload.data();
|
||||||
|
packet.dwPacketMode = packet_mode;
|
||||||
|
callback(real_handle, &packet, user);
|
||||||
|
}
|
||||||
|
|
||||||
|
void emitIFrame(
|
||||||
|
const LONG real_handle = g_real_handle,
|
||||||
|
const DWORD timestamp_low = 0,
|
||||||
|
const DWORD timestamp_high = 0,
|
||||||
|
const DWORD frame_number = 0,
|
||||||
|
const DWORD frame_rate = 0,
|
||||||
|
const DWORD packet_mode = 0)
|
||||||
|
{
|
||||||
|
emitEsPacket(
|
||||||
|
kVideoIFrame,
|
||||||
|
real_handle,
|
||||||
|
{0x00, 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x21},
|
||||||
|
640,
|
||||||
|
360,
|
||||||
|
timestamp_low,
|
||||||
|
timestamp_high,
|
||||||
|
frame_number,
|
||||||
|
frame_rate,
|
||||||
|
packet_mode);
|
||||||
|
}
|
||||||
|
|
||||||
|
void emitPFrame(const LONG real_handle = g_real_handle)
|
||||||
|
{
|
||||||
|
emitEsPacket(
|
||||||
|
kVideoPFrame,
|
||||||
|
real_handle,
|
||||||
|
{0x00, 0x00, 0x00, 0x01, 0x41, 0x9A, 0x20});
|
||||||
|
}
|
||||||
|
|
||||||
|
bool check(const bool condition, const char* expression, const int line)
|
||||||
|
{
|
||||||
|
if (condition) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
std::cerr << "CHECK failed at line " << line << ": " << expression << '\n';
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#define CHECK_TRUE(expression) \
|
||||||
|
do { \
|
||||||
|
if (!check(static_cast<bool>(expression), #expression, __LINE__)) { \
|
||||||
|
return false; \
|
||||||
|
} \
|
||||||
|
} while (false)
|
||||||
|
|
||||||
|
cmvr::config::HikvisionCameraConfig makeConfig()
|
||||||
|
{
|
||||||
|
cmvr::config::HikvisionCameraConfig config;
|
||||||
|
config.set_id("hikvision_callback_test");
|
||||||
|
config.set_ip("127.0.0.1");
|
||||||
|
config.set_username("admin");
|
||||||
|
config.set_password("test-only");
|
||||||
|
config.set_port(8000);
|
||||||
|
config.set_channel(1);
|
||||||
|
config.set_stream_type(0);
|
||||||
|
config.set_link_mode(0);
|
||||||
|
config.set_width(1920);
|
||||||
|
config.set_height(1080);
|
||||||
|
config.set_fps(25);
|
||||||
|
config.set_codec("H264");
|
||||||
|
config.set_buffer_size(32);
|
||||||
|
return config;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool testCallbackPublicationLifecycle()
|
||||||
|
{
|
||||||
|
resetFakeSdk();
|
||||||
|
cmvr::device::HikvisionCamera camera(makeConfig());
|
||||||
|
CHECK_TRUE(camera.init());
|
||||||
|
CHECK_TRUE(camera.start());
|
||||||
|
CHECK_TRUE(camera.startStreaming());
|
||||||
|
CHECK_TRUE(g_key_frame_request_count.load() == 1);
|
||||||
|
|
||||||
|
size_t cursor = 0;
|
||||||
|
cmvr::device::StreamFrameData frame;
|
||||||
|
|
||||||
|
// A new subscriber must not receive an undecodable inter frame, and a
|
||||||
|
// delayed callback from an older preview handle must also be rejected.
|
||||||
|
emitPFrame();
|
||||||
|
emitIFrame(g_real_handle - 1);
|
||||||
|
CHECK_TRUE(!camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(10)));
|
||||||
|
|
||||||
|
emitIFrame(
|
||||||
|
g_real_handle,
|
||||||
|
0x89ABCDEFU,
|
||||||
|
0x01234567U,
|
||||||
|
42U,
|
||||||
|
30U,
|
||||||
|
1U);
|
||||||
|
CHECK_TRUE(camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(50)));
|
||||||
|
CHECK_TRUE(frame.stream_epoch == 1);
|
||||||
|
CHECK_TRUE(frame.sequence == 0);
|
||||||
|
CHECK_TRUE(frame.codec_config_generation == 1);
|
||||||
|
CHECK_TRUE(frame.bKey);
|
||||||
|
CHECK_TRUE(frame.width == 640);
|
||||||
|
CHECK_TRUE(frame.height == 360);
|
||||||
|
CHECK_TRUE(frame.source_timestamp == 0x0123456789ABCDEFULL);
|
||||||
|
CHECK_TRUE(frame.source_frame_number == 42U);
|
||||||
|
CHECK_TRUE(frame.fps == 30);
|
||||||
|
CHECK_TRUE(frame.time_base_den == 30);
|
||||||
|
|
||||||
|
CHECK_TRUE(camera.requestKeyFrame());
|
||||||
|
CHECK_TRUE(g_key_frame_request_count.load() == 2);
|
||||||
|
|
||||||
|
CHECK_TRUE(camera.controlPtz(
|
||||||
|
cmvr::device::PtzCommand::PanLeft, false, 99));
|
||||||
|
CHECK_TRUE(g_ptz_call_count.load() == 1);
|
||||||
|
CHECK_TRUE(g_last_ptz_command.load() == PAN_LEFT);
|
||||||
|
CHECK_TRUE(g_last_ptz_stop.load() == 0);
|
||||||
|
CHECK_TRUE(g_last_ptz_speed.load() == 7);
|
||||||
|
|
||||||
|
std::string json_response;
|
||||||
|
CHECK_TRUE(camera.executeJsonCommand(
|
||||||
|
R"({"command":"ptz","direction":"zoom_in","speed":3})",
|
||||||
|
json_response));
|
||||||
|
CHECK_TRUE(json_response.find(R"("success":true)") != std::string::npos);
|
||||||
|
CHECK_TRUE(g_ptz_call_count.load() == 2);
|
||||||
|
CHECK_TRUE(g_last_ptz_command.load() == ZOOM_IN);
|
||||||
|
CHECK_TRUE(g_last_ptz_speed.load() == 3);
|
||||||
|
|
||||||
|
// A changed SDK file header invalidates the descriptor generation, but is
|
||||||
|
// not exposed as decoder config until its vendor-specific format is known.
|
||||||
|
emitEsPacket(kFileHeader, g_real_handle, {0x01, 0x02, 0x03});
|
||||||
|
emitPFrame();
|
||||||
|
CHECK_TRUE(!camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(10)));
|
||||||
|
emitIFrame(
|
||||||
|
g_real_handle,
|
||||||
|
0x76543210U,
|
||||||
|
0xFEDCBA98U,
|
||||||
|
99U,
|
||||||
|
1001U,
|
||||||
|
0U);
|
||||||
|
CHECK_TRUE(camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(50)));
|
||||||
|
CHECK_TRUE(frame.sequence == 1);
|
||||||
|
CHECK_TRUE(frame.codec_config_generation == 2);
|
||||||
|
CHECK_TRUE(frame.codec_config.empty());
|
||||||
|
CHECK_TRUE(frame.source_timestamp == 0xFEDCBA9876543210ULL);
|
||||||
|
CHECK_TRUE(frame.source_frame_number == 99U);
|
||||||
|
CHECK_TRUE(frame.fps == 25);
|
||||||
|
CHECK_TRUE(frame.time_base_den == 25);
|
||||||
|
|
||||||
|
constexpr int kConcurrentCallbacks = 8;
|
||||||
|
std::vector<std::thread> producers;
|
||||||
|
producers.reserve(kConcurrentCallbacks);
|
||||||
|
for (int i = 0; i < kConcurrentCallbacks; ++i) {
|
||||||
|
producers.emplace_back(emitPFrame, g_real_handle);
|
||||||
|
}
|
||||||
|
for (auto& producer : producers) {
|
||||||
|
producer.join();
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t latest_cursor = 0;
|
||||||
|
cmvr::device::StreamFrameData latest;
|
||||||
|
CHECK_TRUE(camera.getLatestEncodedFrame(latest, latest_cursor));
|
||||||
|
CHECK_TRUE(latest.stream_epoch == 1);
|
||||||
|
CHECK_TRUE(latest.sequence == static_cast<uint64_t>(kConcurrentCallbacks + 1));
|
||||||
|
CHECK_TRUE(latest_cursor == static_cast<size_t>(kConcurrentCallbacks + 2));
|
||||||
|
|
||||||
|
for (int sequence = 2; sequence <= kConcurrentCallbacks + 1; ++sequence) {
|
||||||
|
CHECK_TRUE(camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(50)));
|
||||||
|
CHECK_TRUE(frame.stream_epoch == 1);
|
||||||
|
CHECK_TRUE(frame.sequence == static_cast<uint64_t>(sequence));
|
||||||
|
}
|
||||||
|
|
||||||
|
camera.stopStreaming();
|
||||||
|
emitIFrame();
|
||||||
|
CHECK_TRUE(!camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(10)));
|
||||||
|
|
||||||
|
CHECK_TRUE(camera.startStreaming());
|
||||||
|
CHECK_TRUE(g_key_frame_request_count.load() == 3);
|
||||||
|
emitPFrame();
|
||||||
|
emitIFrame(g_real_handle - 1);
|
||||||
|
CHECK_TRUE(!camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(10)));
|
||||||
|
emitIFrame();
|
||||||
|
CHECK_TRUE(camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(50)));
|
||||||
|
CHECK_TRUE(frame.stream_epoch == 2);
|
||||||
|
CHECK_TRUE(frame.sequence == 0);
|
||||||
|
CHECK_TRUE(frame.codec_config_generation == 3);
|
||||||
|
|
||||||
|
// The fake StopRealPlay invokes the SDK callback synchronously. stop()
|
||||||
|
// owns ctrl_mtx_ here, proving the callback neither takes that mutex nor
|
||||||
|
// publishes after the preview handle has been invalidated.
|
||||||
|
CHECK_TRUE(camera.stop());
|
||||||
|
CHECK_TRUE(g_stop_callback_count.load() == 1);
|
||||||
|
CHECK_TRUE(!camera.waitEncodedFrame(
|
||||||
|
frame, cursor, std::chrono::milliseconds(10)));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
|
||||||
|
BOOL NET_DVR_Init()
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_Cleanup()
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_SetConnectTime(DWORD, DWORD)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_SetReconnect(DWORD, BOOL)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_SetLogToFile(DWORD, char*, BOOL)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
LONG NET_DVR_Login_V40(
|
||||||
|
LPNET_DVR_USER_LOGIN_INFO,
|
||||||
|
LPNET_DVR_DEVICEINFO_V40)
|
||||||
|
{
|
||||||
|
return 7;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_Logout(LONG)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
DWORD NET_DVR_GetLastError()
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
LONG NET_DVR_RealPlay_V40(
|
||||||
|
LONG,
|
||||||
|
LPNET_DVR_PREVIEWINFO,
|
||||||
|
REALDATACALLBACK,
|
||||||
|
void*)
|
||||||
|
{
|
||||||
|
return g_real_handle;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_SetESRealPlayCallBack(
|
||||||
|
LONG,
|
||||||
|
EsDataCallback callback,
|
||||||
|
void* user)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(g_fake_sdk_mutex);
|
||||||
|
g_es_data_callback = callback;
|
||||||
|
g_es_data_user = user;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_StopRealPlay(const LONG real_handle)
|
||||||
|
{
|
||||||
|
EsDataCallback callback = nullptr;
|
||||||
|
void* user = nullptr;
|
||||||
|
{
|
||||||
|
std::lock_guard lock(g_fake_sdk_mutex);
|
||||||
|
callback = g_es_data_callback;
|
||||||
|
user = g_es_data_user;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (callback) {
|
||||||
|
std::vector<BYTE> idr{
|
||||||
|
0x00, 0x00, 0x00, 0x01, 0x65, 0x88, 0x84, 0x21
|
||||||
|
};
|
||||||
|
NET_DVR_PACKET_INFO_EX packet{};
|
||||||
|
packet.wWidth = 640;
|
||||||
|
packet.wHeight = 360;
|
||||||
|
packet.dwPacketType = kVideoIFrame;
|
||||||
|
packet.dwPacketSize = static_cast<DWORD>(idr.size());
|
||||||
|
packet.pPacketBuffer = idr.data();
|
||||||
|
callback(real_handle, &packet, user);
|
||||||
|
++g_stop_callback_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
std::lock_guard lock(g_fake_sdk_mutex);
|
||||||
|
g_es_data_callback = nullptr;
|
||||||
|
g_es_data_user = nullptr;
|
||||||
|
}
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_SaveRealData(LONG, char*)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_StopSaveRealData(LONG)
|
||||||
|
{
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_MakeKeyFrame(LONG, LONG)
|
||||||
|
{
|
||||||
|
++g_key_frame_request_count;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_MakeKeyFrameSub(LONG, LONG)
|
||||||
|
{
|
||||||
|
++g_key_frame_request_count;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
BOOL NET_DVR_PTZControlWithSpeed_Other(
|
||||||
|
LONG,
|
||||||
|
LONG,
|
||||||
|
const DWORD command,
|
||||||
|
const DWORD stop,
|
||||||
|
const DWORD speed)
|
||||||
|
{
|
||||||
|
++g_ptz_call_count;
|
||||||
|
g_last_ptz_command = command;
|
||||||
|
g_last_ptz_stop = stop;
|
||||||
|
g_last_ptz_speed = speed;
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // extern "C"
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
if (!testCallbackPublicationLifecycle()) {
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
std::cout << "hikvision_camera_callback_test: PASS\n";
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@ -30,6 +30,7 @@ namespace cmvr::device
|
|||||||
bool init() override;
|
bool init() override;
|
||||||
bool start() override;
|
bool start() override;
|
||||||
bool stop() override;
|
bool stop() override;
|
||||||
|
void getState(CameraState& state) override;
|
||||||
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
||||||
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
||||||
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
||||||
|
|||||||
@ -50,6 +50,11 @@ bool MechmindCamera::init() {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void MechmindCamera::getState(CameraState& state) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
|
state = state_;
|
||||||
|
}
|
||||||
|
|
||||||
bool MechmindCamera::start() {
|
bool MechmindCamera::start() {
|
||||||
try {
|
try {
|
||||||
std::lock_guard lock(dev_mtx_);
|
std::lock_guard lock(dev_mtx_);
|
||||||
@ -68,6 +73,7 @@ bool MechmindCamera::start() {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
catch (const std::exception& e) {
|
catch (const std::exception& e) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
const string error_msg = "[MechmindCamera] (start): " + string(e.what());
|
const string error_msg = "[MechmindCamera] (start): " + string(e.what());
|
||||||
CMVR_LOG(ERROR) << error_msg;
|
CMVR_LOG(ERROR) << error_msg;
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
@ -85,6 +91,7 @@ bool MechmindCamera::stop() {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
catch (const exception &e) {
|
catch (const exception &e) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
const string error_msg = "[MechmindCamera] (stop): " + string(e.what());
|
const string error_msg = "[MechmindCamera] (stop): " + string(e.what());
|
||||||
CMVR_LOG(ERROR) << error_msg;
|
CMVR_LOG(ERROR) << error_msg;
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
@ -141,6 +148,7 @@ void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
catch (const exception &e) {
|
catch (const exception &e) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what());
|
const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what());
|
||||||
CMVR_LOG(ERROR) << error_msg;
|
CMVR_LOG(ERROR) << error_msg;
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
@ -175,6 +183,7 @@ void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
|
|||||||
depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data());
|
depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data());
|
||||||
}
|
}
|
||||||
catch (const exception &e) {
|
catch (const exception &e) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what());
|
const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what());
|
||||||
CMVR_LOG(ERROR) << error_msg;
|
CMVR_LOG(ERROR) << error_msg;
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
@ -258,6 +267,7 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics
|
|||||||
textured_plc_to_rgbd_(textured_pcl, color, depth);
|
textured_plc_to_rgbd_(textured_pcl, color, depth);
|
||||||
}
|
}
|
||||||
catch (const exception &e) {
|
catch (const exception &e) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what());
|
const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what());
|
||||||
CMVR_LOG(ERROR) << error_msg;
|
CMVR_LOG(ERROR) << error_msg;
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
@ -269,12 +279,14 @@ void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics
|
|||||||
}
|
}
|
||||||
|
|
||||||
void MechmindCamera::startRecording(const std::string& video_path) {
|
void MechmindCamera::startRecording(const std::string& video_path) {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
state_.error_message = "startRecording is not implemented";
|
state_.error_message = "startRecording is not implemented";
|
||||||
CMVR_LOG(ERROR) << "[MechmindCamera] (startRecording): " << state_.error_message;
|
CMVR_LOG(ERROR) << "[MechmindCamera] (startRecording): " << state_.error_message;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MechmindCamera::stopRecording() {
|
void MechmindCamera::stopRecording() {
|
||||||
|
std::lock_guard lock(dev_mtx_);
|
||||||
state_.is_error = true;
|
state_.is_error = true;
|
||||||
state_.error_message = "stopRecording is not implemented";
|
state_.error_message = "stopRecording is not implemented";
|
||||||
CMVR_LOG(ERROR) << "[MechmindCamera] (stopRecording): " << state_.error_message;
|
CMVR_LOG(ERROR) << "[MechmindCamera] (stopRecording): " << state_.error_message;
|
||||||
|
|||||||
@ -39,6 +39,7 @@ public:
|
|||||||
bool init() override;
|
bool init() override;
|
||||||
bool start() override;
|
bool start() override;
|
||||||
bool stop() override;
|
bool stop() override;
|
||||||
|
void getState(CameraState& state) override;
|
||||||
|
|
||||||
void setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn);
|
void setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn);
|
||||||
void setFovyDeg(double fovy_deg);
|
void setFovyDeg(double fovy_deg);
|
||||||
|
|||||||
@ -52,6 +52,12 @@ MujocoCamera::~MujocoCamera()
|
|||||||
stop();
|
stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void MujocoCamera::getState(CameraState& state)
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
state = state_;
|
||||||
|
}
|
||||||
|
|
||||||
bool MujocoCamera::init()
|
bool MujocoCamera::init()
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(mtx_);
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
|||||||
@ -24,6 +24,7 @@ namespace cmvr::device{
|
|||||||
bool init() override;
|
bool init() override;
|
||||||
bool start() override;
|
bool start() override;
|
||||||
bool stop() override;
|
bool stop() override;
|
||||||
|
void getState(CameraState& state) override;
|
||||||
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
||||||
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
||||||
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
||||||
|
|||||||
@ -143,6 +143,12 @@ RealsenseCamera::~RealsenseCamera() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void RealsenseCamera::getState(CameraState& state)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
state = state_;
|
||||||
|
}
|
||||||
|
|
||||||
bool RealsenseCamera::init() {
|
bool RealsenseCamera::init() {
|
||||||
try {
|
try {
|
||||||
std::lock_guard lock(ctrl_mtx_);
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
|||||||
@ -29,6 +29,7 @@ namespace cmvr::device {
|
|||||||
bool init() override;
|
bool init() override;
|
||||||
bool start() override;
|
bool start() override;
|
||||||
bool stop() override;
|
bool stop() override;
|
||||||
|
void getState(CameraState& state) override;
|
||||||
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
|
||||||
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
|
||||||
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) override;
|
||||||
|
|||||||
@ -57,6 +57,12 @@ UVCCamera::~UVCCamera() {
|
|||||||
recording_thread_->join();
|
recording_thread_->join();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void UVCCamera::getState(CameraState& state)
|
||||||
|
{
|
||||||
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
|
state = state_;
|
||||||
|
}
|
||||||
|
|
||||||
bool UVCCamera::init() {
|
bool UVCCamera::init() {
|
||||||
std::lock_guard lock(ctrl_mtx_);
|
std::lock_guard lock(ctrl_mtx_);
|
||||||
clear_error_();
|
clear_error_();
|
||||||
|
|||||||
@ -1,7 +1,9 @@
|
|||||||
#ifndef CMVR_ES_DEVICE_TYPES_H
|
#ifndef CMVR_ES_DEVICE_TYPES_H
|
||||||
#define CMVR_ES_DEVICE_TYPES_H
|
#define CMVR_ES_DEVICE_TYPES_H
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
namespace cmvr::device {
|
namespace cmvr::device {
|
||||||
|
|
||||||
@ -68,6 +70,87 @@ namespace cmvr::device {
|
|||||||
std::string type_name;
|
std::string type_name;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// DeviceManager lifecycle and device-reported health are deliberately
|
||||||
|
// separate. A device can, for example, be READY from the manager's point
|
||||||
|
// of view while its backend has not implemented health reporting yet.
|
||||||
|
enum class ManagedDeviceState {
|
||||||
|
Unknown,
|
||||||
|
Disabled,
|
||||||
|
Initializing,
|
||||||
|
Registered,
|
||||||
|
Ready,
|
||||||
|
Running,
|
||||||
|
Stopped,
|
||||||
|
Error,
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class DeviceHealthState {
|
||||||
|
Unknown,
|
||||||
|
Healthy,
|
||||||
|
Degraded,
|
||||||
|
Fault,
|
||||||
|
};
|
||||||
|
|
||||||
|
inline std::string toString(ManagedDeviceState state) {
|
||||||
|
switch (state) {
|
||||||
|
case ManagedDeviceState::Disabled:
|
||||||
|
return "Disabled";
|
||||||
|
case ManagedDeviceState::Initializing:
|
||||||
|
return "Initializing";
|
||||||
|
case ManagedDeviceState::Registered:
|
||||||
|
return "Registered";
|
||||||
|
case ManagedDeviceState::Ready:
|
||||||
|
return "Ready";
|
||||||
|
case ManagedDeviceState::Running:
|
||||||
|
return "Running";
|
||||||
|
case ManagedDeviceState::Stopped:
|
||||||
|
return "Stopped";
|
||||||
|
case ManagedDeviceState::Error:
|
||||||
|
return "Error";
|
||||||
|
case ManagedDeviceState::Unknown:
|
||||||
|
default:
|
||||||
|
return "Unknown";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::string toString(DeviceHealthState state) {
|
||||||
|
switch (state) {
|
||||||
|
case DeviceHealthState::Healthy:
|
||||||
|
return "Healthy";
|
||||||
|
case DeviceHealthState::Degraded:
|
||||||
|
return "Degraded";
|
||||||
|
case DeviceHealthState::Fault:
|
||||||
|
return "Fault";
|
||||||
|
case DeviceHealthState::Unknown:
|
||||||
|
default:
|
||||||
|
return "Unknown";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct DeviceHealthSnapshot {
|
||||||
|
DeviceHealthState state = DeviceHealthState::Unknown;
|
||||||
|
std::string error_message;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct ManagedDeviceSnapshot {
|
||||||
|
std::string id;
|
||||||
|
DeviceKind kind = DeviceKind::Unknown;
|
||||||
|
std::string type_name;
|
||||||
|
bool enabled = false;
|
||||||
|
ManagedDeviceState state = ManagedDeviceState::Unknown;
|
||||||
|
DeviceHealthSnapshot health;
|
||||||
|
bool abnormal = false;
|
||||||
|
std::string error_message;
|
||||||
|
std::uint64_t status_updated_at_unix_ms = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct DeviceManagerSnapshot {
|
||||||
|
std::string name;
|
||||||
|
std::string version;
|
||||||
|
std::string description;
|
||||||
|
std::vector<ManagedDeviceSnapshot> devices;
|
||||||
|
};
|
||||||
|
|
||||||
} // namespace cmvr::device
|
} // namespace cmvr::device
|
||||||
|
|
||||||
#endif // CMVR_ES_DEVICE_TYPES_H
|
#endif // CMVR_ES_DEVICE_TYPES_H
|
||||||
|
|||||||
@ -130,6 +130,24 @@ namespace cmvr::device {
|
|||||||
virtual Status state() const = 0;
|
virtual Status state() const = 0;
|
||||||
virtual std::string lastError() const = 0;
|
virtual std::string lastError() const = 0;
|
||||||
|
|
||||||
|
DeviceHealthSnapshot healthSnapshot() override {
|
||||||
|
const auto lifecycle = state();
|
||||||
|
const auto error = lastError();
|
||||||
|
|
||||||
|
DeviceHealthSnapshot health;
|
||||||
|
health.error_message = error;
|
||||||
|
if (lifecycle == Status::FAULT) {
|
||||||
|
health.state = DeviceHealthState::Fault;
|
||||||
|
} else if (!error.empty()) {
|
||||||
|
health.state = DeviceHealthState::Degraded;
|
||||||
|
} else if (lifecycle == Status::INITIALIZED ||
|
||||||
|
lifecycle == Status::STREAMING ||
|
||||||
|
lifecycle == Status::STOPPED) {
|
||||||
|
health.state = DeviceHealthState::Healthy;
|
||||||
|
}
|
||||||
|
return health;
|
||||||
|
}
|
||||||
|
|
||||||
virtual void getState(DexHandState& state) {
|
virtual void getState(DexHandState& state) {
|
||||||
state = DexHandState{};
|
state = DexHandState{};
|
||||||
const auto lifecycle = this->state();
|
const auto lifecycle = this->state();
|
||||||
|
|||||||
@ -10,4 +10,4 @@ target_link_libraries(microphone
|
|||||||
cmvr_es::proto
|
cmvr_es::proto
|
||||||
)
|
)
|
||||||
|
|
||||||
add_library(cmvr_es::device::microphone ALIAS microphone)
|
add_library(cmvr_es::device::microphone ALIAS microphone)
|
||||||
@ -6,6 +6,9 @@
|
|||||||
#define CMVR_ES_ABSTRACT_MICROPHONE_H
|
#define CMVR_ES_ABSTRACT_MICROPHONE_H
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstddef>
|
||||||
|
|
||||||
#include "devices/abstract_device.h"
|
#include "devices/abstract_device.h"
|
||||||
#include "cmvr/config/microphone_config/microphone_config.pb.h"
|
#include "cmvr/config/microphone_config/microphone_config.pb.h"
|
||||||
namespace cmvr::device{
|
namespace cmvr::device{
|
||||||
@ -16,12 +19,41 @@ namespace cmvr::device{
|
|||||||
|
|
||||||
DeviceKind kind() const noexcept override { return DeviceKind::Microphone; }
|
DeviceKind kind() const noexcept override { return DeviceKind::Microphone; }
|
||||||
virtual void getState(MicrophoneState &state) {}
|
virtual void getState(MicrophoneState &state) {}
|
||||||
|
DeviceHealthSnapshot healthSnapshot() override {
|
||||||
|
MicrophoneState state{};
|
||||||
|
getState(state);
|
||||||
|
|
||||||
|
DeviceHealthSnapshot health;
|
||||||
|
health.error_message = state.error_message;
|
||||||
|
if (state.is_error) {
|
||||||
|
health.state = DeviceHealthState::Fault;
|
||||||
|
} else if (!state.error_message.empty()) {
|
||||||
|
health.state = DeviceHealthState::Degraded;
|
||||||
|
} else if (state.is_initialized) {
|
||||||
|
health.state = DeviceHealthState::Healthy;
|
||||||
|
}
|
||||||
|
return health;
|
||||||
|
}
|
||||||
virtual void startRecording(const std::string& outputFilePath) {}
|
virtual void startRecording(const std::string& outputFilePath) {}
|
||||||
virtual void stopRecording() {}
|
virtual void stopRecording() {}
|
||||||
virtual void pause() {}
|
virtual void pause() {}
|
||||||
virtual void resume() {}
|
virtual void resume() {}
|
||||||
virtual void setVolume(const int volume) {}
|
virtual void setVolume(const int volume) {}
|
||||||
virtual int getVolume() {return 0;}
|
virtual int getVolume() {return 0;}
|
||||||
|
virtual bool startStreaming() {return true;}
|
||||||
|
virtual void stopStreaming() {}
|
||||||
|
virtual void getEncodedFrame(AudioStreamFrameData& frame_data, size_t& index) {}
|
||||||
|
virtual bool waitEncodedFrame(
|
||||||
|
AudioStreamFrameData& frame_data,
|
||||||
|
size_t& index,
|
||||||
|
std::chrono::milliseconds timeout) {
|
||||||
|
(void)timeout;
|
||||||
|
getEncodedFrame(frame_data, index);
|
||||||
|
return !frame_data.data.empty();
|
||||||
|
}
|
||||||
|
virtual bool getLatestEncodedFrame(AudioStreamFrameData& frame_data, size_t& next_index) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
MicrophoneState state_{};
|
MicrophoneState state_{};
|
||||||
|
|||||||
@ -3,26 +3,30 @@
|
|||||||
#ifndef CMVR_ES_FFMPEG_MICROPHONE_H
|
#ifndef CMVR_ES_FFMPEG_MICROPHONE_H
|
||||||
#define CMVR_ES_FFMPEG_MICROPHONE_H
|
#define CMVR_ES_FFMPEG_MICROPHONE_H
|
||||||
|
|
||||||
#include <thread>
|
#include <atomic>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <memory>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <queue>
|
#include <queue>
|
||||||
#include <condition_variable>
|
#include <thread>
|
||||||
#include "microphone/abstract_microphone.h"
|
|
||||||
#include <boost/lockfree/spsc_queue.hpp>
|
#include <boost/lockfree/spsc_queue.hpp>
|
||||||
|
|
||||||
|
#include "common/base/ring_buffer.h"
|
||||||
|
#include "microphone/abstract_microphone.h"
|
||||||
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
|
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
|
||||||
|
|
||||||
namespace cmvr::device {
|
namespace cmvr::device {
|
||||||
|
|
||||||
class ffmpegMicroPhone final : public AbstractMicrophone {
|
class ffmpegMicroPhone final : public AbstractMicrophone {
|
||||||
public:
|
public:
|
||||||
// 录制状态
|
|
||||||
enum class RecordingState {
|
enum class RecordingState {
|
||||||
STOPPED,
|
STOPPED,
|
||||||
RECORDING,
|
RECORDING,
|
||||||
PAUSED
|
PAUSED
|
||||||
};
|
};
|
||||||
public:
|
|
||||||
ffmpegMicroPhone(const config::FFMpegMicroPhoneConfig& cfg);
|
explicit ffmpegMicroPhone(const config::FFMpegMicroPhoneConfig& cfg);
|
||||||
~ffmpegMicroPhone() override;
|
~ffmpegMicroPhone() override;
|
||||||
std::string typeName() const override { return "FFMpegMicroPhone"; }
|
std::string typeName() const override { return "FFMpegMicroPhone"; }
|
||||||
bool init() override;
|
bool init() override;
|
||||||
@ -36,49 +40,64 @@ namespace cmvr::device {
|
|||||||
|
|
||||||
void setVolume(const int volume) override;
|
void setVolume(const int volume) override;
|
||||||
int getVolume() override;
|
int getVolume() override;
|
||||||
|
bool startStreaming() override;
|
||||||
|
void stopStreaming() override;
|
||||||
|
void getEncodedFrame(AudioStreamFrameData& frame_data, size_t& index) override;
|
||||||
|
bool waitEncodedFrame(AudioStreamFrameData& frame_data, size_t& index, std::chrono::milliseconds timeout) override;
|
||||||
|
bool getLatestEncodedFrame(AudioStreamFrameData& frame_data, size_t& next_index) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// 音频采集和编码线程
|
|
||||||
void audioThread();
|
void audioThread();
|
||||||
|
|
||||||
// 初始化FFmpeg采集和编码
|
|
||||||
bool initFFmpeg();
|
bool initFFmpeg();
|
||||||
|
|
||||||
// 关闭FFmpeg资源
|
|
||||||
void closeFFmpeg();
|
void closeFFmpeg();
|
||||||
|
bool startCapture_(bool write_file);
|
||||||
|
void stopCapture_();
|
||||||
|
bool recoverStreamingCapture_(int read_error);
|
||||||
|
static int interruptCallback_(void* opaque);
|
||||||
|
AudioStreamFormat currentStreamFormat_() const;
|
||||||
|
void pushEncodedPacket_(const AVPacket* packet);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// FFmpeg采集相关
|
|
||||||
AVFormatContext* input_fmt_ctx;
|
AVFormatContext* input_fmt_ctx;
|
||||||
AVCodecContext* input_codec_ctx;
|
AVCodecContext* input_codec_ctx;
|
||||||
AVFrame* input_frame;
|
AVFrame* input_frame;
|
||||||
|
|
||||||
// FFmpeg编码相关
|
|
||||||
AVFormatContext* output_fmt_ctx;
|
AVFormatContext* output_fmt_ctx;
|
||||||
AVOutputFormat* output_fmt;
|
const AVOutputFormat* output_fmt;
|
||||||
AVStream* audio_st;
|
AVStream* audio_st;
|
||||||
AVCodecContext* audio_codec_ctx;
|
AVCodecContext* audio_codec_ctx;
|
||||||
AVCodec* audio_codec;
|
const AVCodec* audio_codec;
|
||||||
SwrContext* swr_ctx;
|
SwrContext* swr_ctx;
|
||||||
AVFrame* audio_frame;
|
AVFrame* audio_frame;
|
||||||
|
|
||||||
std::string output_file;
|
std::string output_file;
|
||||||
std::string format_name;
|
std::string format_name;
|
||||||
std::atomic<bool> is_recording;
|
std::atomic<bool> is_capturing_{false};
|
||||||
|
std::atomic<bool> interrupt_requested_{false};
|
||||||
|
std::atomic<bool> write_output_file_{false};
|
||||||
std::atomic<bool> is_paused;
|
std::atomic<bool> is_paused;
|
||||||
std::mutex pause_mutex;
|
std::mutex pause_mutex;
|
||||||
std::condition_variable pause_cv;
|
std::condition_variable pause_cv;
|
||||||
|
|
||||||
// 音频参数
|
|
||||||
int sample_rate_;
|
int sample_rate_;
|
||||||
int channels_;
|
int channels_;
|
||||||
int64_t next_pts;
|
int64_t next_pts;
|
||||||
|
|
||||||
private:
|
|
||||||
std::string input_device_;
|
std::string input_device_;
|
||||||
|
|
||||||
std::shared_ptr<std::thread> audio_thread_;
|
std::shared_ptr<std::thread> audio_thread_;
|
||||||
|
// Serializes FFmpeg context start/stop and keeps a new shared lease from
|
||||||
|
// starting until the previous capture thread has exited and been joined.
|
||||||
|
std::mutex capture_mutex_;
|
||||||
std::mutex mtx_;
|
std::mutex mtx_;
|
||||||
|
std::shared_ptr<SPMCRingBuffer<AudioStreamFrameData>> stream_frame_buffer_;
|
||||||
|
// Protected by mtx_. The audio thread snapshots these values while it
|
||||||
|
// assigns metadata to an encoded frame.
|
||||||
|
int stream_count_ = 0;
|
||||||
|
size_t buffer_size_ = 256;
|
||||||
|
uint64_t stream_epoch_ = 0;
|
||||||
|
uint64_t stream_sequence_ = 0;
|
||||||
|
uint32_t codec_config_generation_ = 0;
|
||||||
|
|
||||||
config::FFMpegMicroPhoneConfig config_;
|
config::FFMpegMicroPhoneConfig config_;
|
||||||
};
|
};
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -21,6 +21,8 @@ namespace cmvr::device{
|
|||||||
virtual int getVolume() const {return 0;}
|
virtual int getVolume() const {return 0;}
|
||||||
virtual void pause() {}
|
virtual void pause() {}
|
||||||
virtual void resume() {}
|
virtual void resume() {}
|
||||||
|
virtual bool pushAudioFrame(const AudioStreamFrameData& frame_data) { return false; }
|
||||||
|
virtual void stopStreaming() {}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SpeakerState state_{};
|
SpeakerState state_{};
|
||||||
|
|||||||
@ -114,291 +114,6 @@ enum class ImageType {
|
|||||||
DEPTH, // 深度图像
|
DEPTH, // 深度图像
|
||||||
GRAYSCALE // 灰度图像
|
GRAYSCALE // 灰度图像
|
||||||
};
|
};
|
||||||
|
|
||||||
class HEVCEncoder {
|
|
||||||
public:
|
|
||||||
HEVCEncoder(int width, int height, int fps, int bitrate,
|
|
||||||
const std::string& deviceName = "/dev/dri/renderD128",
|
|
||||||
ImageType imageType = ImageType::COLOR)
|
|
||||||
: width_(width), height_(height), fps_(fps), bitrate_(bitrate),
|
|
||||||
deviceName_(deviceName), imageType_(imageType) {
|
|
||||||
initialized_ = init();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool encodeFrame(const cv::Mat& image) {
|
|
||||||
if (!initialized_ || !codec_ctx_ || !frame_) {
|
|
||||||
CMVR_LOG(ERROR) << "HEVC encoder is not initialized";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (!prepareFrame(image)) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to prepare frame";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (avcodec_send_frame(codec_ctx_.get(), frame_.get()) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Error sending frame to encoder";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<uint8_t> receivePacket(bool& is_key) {
|
|
||||||
std::vector<uint8_t> packetData;
|
|
||||||
if (!initialized_ || !codec_ctx_) {
|
|
||||||
CMVR_LOG(ERROR) << "HEVC encoder is not initialized";
|
|
||||||
return packetData;
|
|
||||||
}
|
|
||||||
FFmpegPtr<AVPacket> packet(av_packet_alloc());
|
|
||||||
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
int ret = avcodec_receive_packet(codec_ctx_.get(), packet.get());
|
|
||||||
if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
|
|
||||||
break;
|
|
||||||
} else if (ret < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Error during encoding";
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
is_key = (packet->flags & AV_PKT_FLAG_KEY) != 0;
|
|
||||||
// 预留足够空间，避免多次内存分配
|
|
||||||
packetData.reserve(packetData.size() + packet->size);
|
|
||||||
// 复制数据包内容到输出向量
|
|
||||||
packetData.insert(packetData.end(),
|
|
||||||
packet->data,
|
|
||||||
packet->data + packet->size);
|
|
||||||
av_packet_unref(packet.get());
|
|
||||||
}
|
|
||||||
return packetData;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
bool init() {
|
|
||||||
// 1. 创建硬件设备上下文
|
|
||||||
AVBufferRef* hw_device_ctx = nullptr;
|
|
||||||
int ret = av_hwdevice_ctx_create(&hw_device_ctx, AV_HWDEVICE_TYPE_VAAPI,
|
|
||||||
deviceName_.c_str(), nullptr, 0);
|
|
||||||
if (ret < 0) {
|
|
||||||
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
|
|
||||||
av_strerror(ret, errbuf, sizeof(errbuf));
|
|
||||||
CMVR_LOG(ERROR) << "Failed to create VAAPI device context: " << errbuf;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
hw_device_ctx_.reset(hw_device_ctx);
|
|
||||||
|
|
||||||
// 2. 查找编码器
|
|
||||||
const AVCodec* codec = avcodec_find_encoder_by_name("hevc_vaapi");
|
|
||||||
if (!codec) {
|
|
||||||
CMVR_LOG(ERROR) << "HEVC VAAPI encoder not found";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 3. 设置编码器上下文
|
|
||||||
codec_ctx_.reset(avcodec_alloc_context3(codec));
|
|
||||||
codec_ctx_->width = width_;
|
|
||||||
codec_ctx_->height = height_;
|
|
||||||
codec_ctx_->time_base = {1, fps_};
|
|
||||||
codec_ctx_->pix_fmt = AV_PIX_FMT_VAAPI;
|
|
||||||
codec_ctx_->bit_rate = bitrate_;
|
|
||||||
codec_ctx_->gop_size = 1;
|
|
||||||
codec_ctx_->hw_device_ctx = av_buffer_ref(hw_device_ctx_.get());
|
|
||||||
|
|
||||||
// 4. 创建硬件帧上下文
|
|
||||||
hw_frames_ctx_.reset(av_hwframe_ctx_alloc(hw_device_ctx_.get()));
|
|
||||||
if (!hw_frames_ctx_) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to allocate hardware frame context";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
AVHWFramesContext* frames_ctx = (AVHWFramesContext*)hw_frames_ctx_->data;
|
|
||||||
frames_ctx->format = AV_PIX_FMT_VAAPI;
|
|
||||||
frames_ctx->sw_format = AV_PIX_FMT_NV12;
|
|
||||||
frames_ctx->width = width_;
|
|
||||||
frames_ctx->height = height_;
|
|
||||||
frames_ctx->initial_pool_size = 20;
|
|
||||||
|
|
||||||
if (av_hwframe_ctx_init(hw_frames_ctx_.get()) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to initialize hardware frame context";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
codec_ctx_->hw_frames_ctx = av_buffer_ref(hw_frames_ctx_.get());
|
|
||||||
|
|
||||||
// 5. 打开编码器
|
|
||||||
if (avcodec_open2(codec_ctx_.get(), codec, nullptr) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Cannot open encoder";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 6. 初始化硬件帧
|
|
||||||
frame_.reset(av_frame_alloc());
|
|
||||||
frame_->format = AV_PIX_FMT_VAAPI;
|
|
||||||
frame_->width = width_;
|
|
||||||
frame_->height = height_;
|
|
||||||
|
|
||||||
if (av_hwframe_get_buffer(hw_frames_ctx_.get(), frame_.get(), 0) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Could not allocate hardware frame data";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 7. 初始化软件帧
|
|
||||||
sw_frame_.reset(av_frame_alloc());
|
|
||||||
sw_frame_->width = width_;
|
|
||||||
sw_frame_->height = height_;
|
|
||||||
sw_frame_->format = (imageType_ == ImageType::DEPTH) ? AV_PIX_FMT_GRAY8 : AV_PIX_FMT_NV12;
|
|
||||||
|
|
||||||
if (av_frame_get_buffer(sw_frame_.get(), 0) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Could not allocate software frame buffer";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool prepareFrame(const cv::Mat& image) {
|
|
||||||
if (imageType_ == ImageType::DEPTH) {
|
|
||||||
return prepareDepthFrame(image);
|
|
||||||
} else {
|
|
||||||
return prepareColorOrGrayscaleFrame(image);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool prepareDepthFrame(const cv::Mat& depthImage) {
|
|
||||||
cv::Mat processedDepth;
|
|
||||||
|
|
||||||
if (depthImage.channels() != 1) {
|
|
||||||
CMVR_LOG(ERROR) << "Depth image must be single-channel";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (depthImage.depth() != CV_8U) {
|
|
||||||
if (depthImage.depth() == CV_16U) {
|
|
||||||
depthImage.convertTo(processedDepth, CV_8U, 255.0 / 65535.0);
|
|
||||||
} else {
|
|
||||||
CMVR_LOG(ERROR) << "Unsupported depth image format";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
processedDepth = depthImage.clone();
|
|
||||||
}
|
|
||||||
|
|
||||||
// 确保图像尺寸匹配
|
|
||||||
if (processedDepth.cols != width_ || processedDepth.rows != height_) {
|
|
||||||
CMVR_LOG(ERROR) << "Depth image size mismatch";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 设置软件帧属性
|
|
||||||
sw_frame_->pts = frame_counter_++;
|
|
||||||
sw_frame_->format = AV_PIX_FMT_GRAY8;
|
|
||||||
av_frame_make_writable(sw_frame_.get());
|
|
||||||
|
|
||||||
// 使用av_image_fill_arrays处理行对齐
|
|
||||||
av_image_fill_arrays(sw_frame_->data, sw_frame_->linesize,
|
|
||||||
processedDepth.data,
|
|
||||||
AV_PIX_FMT_GRAY8,
|
|
||||||
width_, height_, 1);
|
|
||||||
|
|
||||||
// 上传到硬件帧
|
|
||||||
if (av_hwframe_transfer_data(frame_.get(), sw_frame_.get(), 0) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Error transferring depth data to hardware frame";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
frame_->pts = sw_frame_->pts;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool prepareColorOrGrayscaleFrame(const cv::Mat& image) {
|
|
||||||
if (image.cols != width_ || image.rows != height_) {
|
|
||||||
CMVR_LOG(ERROR) << "Image size mismatch: " << image.cols << "x" << image.rows
|
|
||||||
<< " vs " << width_ << "x" << height_;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 1. 确定输入图像的像素格式（OpenCV的Mat格式）
|
|
||||||
AVPixelFormat src_pix_fmt;
|
|
||||||
if (image.channels() == 3) {
|
|
||||||
src_pix_fmt = AV_PIX_FMT_BGR24; // OpenCV默认是BGR格式（3通道）
|
|
||||||
} else if (image.channels() == 1) {
|
|
||||||
src_pix_fmt = AV_PIX_FMT_GRAY8; // 灰度图（1通道）
|
|
||||||
} else {
|
|
||||||
CMVR_LOG(ERROR) << "Unsupported channel count: " << image.channels();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 2. 初始化格式转换上下文（swscale）
|
|
||||||
SwsContext* sws_ctx = sws_getContext(
|
|
||||||
width_, height_, src_pix_fmt, // 源宽高和格式
|
|
||||||
width_, height_, AV_PIX_FMT_NV12, // 目标宽高和格式（NV12）
|
|
||||||
SWS_BILINEAR, // 缩放算法（可根据需求调整）
|
|
||||||
nullptr, nullptr, nullptr
|
|
||||||
);
|
|
||||||
if (!sws_ctx) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to create sws context";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 3. 配置源数据（OpenCV的Mat）
|
|
||||||
uint8_t* src_data[4] = {nullptr};
|
|
||||||
int src_linesize[4] = {0};
|
|
||||||
if (image.channels() == 3) {
|
|
||||||
src_data[0] = image.data; // BGR数据起始地址
|
|
||||||
src_linesize[0] = image.step; // 每行字节数（含OpenCV的对齐填充）
|
|
||||||
} else {
|
|
||||||
src_data[0] = image.data; // 灰度数据起始地址
|
|
||||||
src_linesize[0] = image.step; // 灰度图每行字节数
|
|
||||||
}
|
|
||||||
|
|
||||||
// 4. 配置目标数据（NV12格式的AVFrame）
|
|
||||||
av_frame_make_writable(sw_frame_.get());
|
|
||||||
sw_frame_->format = AV_PIX_FMT_NV12;
|
|
||||||
sw_frame_->width = width_;
|
|
||||||
sw_frame_->height = height_;
|
|
||||||
|
|
||||||
// 5. 执行格式转换（BGR/GRAY -> NV12）
|
|
||||||
int ret = sws_scale(
|
|
||||||
sws_ctx,
|
|
||||||
src_data, // 源图像数据
|
|
||||||
src_linesize, // 源图像每行字节数
|
|
||||||
0, // 起始行
|
|
||||||
height_, // 转换的行数
|
|
||||||
sw_frame_->data, // 目标图像数据（sw_frame_的data指针）
|
|
||||||
sw_frame_->linesize // 目标图像每行字节数
|
|
||||||
);
|
|
||||||
if (ret <= 0) {
|
|
||||||
CMVR_LOG(ERROR) << "sws_scale failed, ret=" << ret;
|
|
||||||
sws_freeContext(sws_ctx);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 6. 释放转换上下文
|
|
||||||
sws_freeContext(sws_ctx);
|
|
||||||
|
|
||||||
// 7. 设置帧属性并上传到硬件
|
|
||||||
sw_frame_->pts = frame_counter_++;
|
|
||||||
if (av_hwframe_transfer_data(frame_.get(), sw_frame_.get(), 0) < 0) {
|
|
||||||
CMVR_LOG(ERROR) << "Error transferring data to hardware frame";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
frame_->pts = sw_frame_->pts;
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
int width_;
|
|
||||||
int height_;
|
|
||||||
int fps_;
|
|
||||||
int bitrate_;
|
|
||||||
int frame_counter_ = 0;
|
|
||||||
std::string deviceName_;
|
|
||||||
ImageType imageType_;
|
|
||||||
bool initialized_{false};
|
|
||||||
|
|
||||||
FFmpegPtr<AVCodecContext> codec_ctx_;
|
|
||||||
FFmpegPtr<AVFrame> frame_;
|
|
||||||
FFmpegPtr<AVFrame> sw_frame_;
|
|
||||||
FFmpegPtr<AVBufferRef> hw_device_ctx_;
|
|
||||||
FFmpegPtr<AVBufferRef> hw_frames_ctx_;
|
|
||||||
};
|
|
||||||
} // namespace ffmpeg
|
} // namespace ffmpeg
|
||||||
|
|
||||||
#endif //FFMPEG_PTR_H
|
#endif //FFMPEG_PTR_H
|
||||||
|
|||||||
@ -3,9 +3,14 @@
|
|||||||
#ifndef CMVR_ES_FFMPEG_SPEAKER_H
|
#ifndef CMVR_ES_FFMPEG_SPEAKER_H
|
||||||
#define CMVR_ES_FFMPEG_SPEAKER_H
|
#define CMVR_ES_FFMPEG_SPEAKER_H
|
||||||
|
|
||||||
#include <thread>
|
#include <atomic>
|
||||||
#include <mutex>
|
#include <chrono>
|
||||||
#include <condition_variable>
|
#include <condition_variable>
|
||||||
|
#include <memory>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
#include "speaker/abstract_speaker.h"
|
#include "speaker/abstract_speaker.h"
|
||||||
#include <boost/lockfree/spsc_queue.hpp>
|
#include <boost/lockfree/spsc_queue.hpp>
|
||||||
#include <pulse/simple.h>
|
#include <pulse/simple.h>
|
||||||
@ -33,6 +38,8 @@ namespace cmvr::device {
|
|||||||
void pause() override;
|
void pause() override;
|
||||||
void resume() override;
|
void resume() override;
|
||||||
void getState(SpeakerState& state) override;
|
void getState(SpeakerState& state) override;
|
||||||
|
bool pushAudioFrame(const AudioStreamFrameData& frame_data) override;
|
||||||
|
void stopStreaming() override;
|
||||||
|
|
||||||
void resetPlayState();
|
void resetPlayState();
|
||||||
bool initPulseDevice_();
|
bool initPulseDevice_();
|
||||||
@ -40,6 +47,12 @@ namespace cmvr::device {
|
|||||||
private:
|
private:
|
||||||
void decode_audio_();
|
void decode_audio_();
|
||||||
void play_audio_();
|
void play_audio_();
|
||||||
|
bool startStreamingPlayback_(const AudioStreamFrameData& frame_data);
|
||||||
|
bool pushPcmFrame_(const AudioStreamFrameData& frame_data);
|
||||||
|
bool decodeStreamFrame_(const AudioStreamFrameData& frame_data);
|
||||||
|
bool initStreamDecoder_(const AudioStreamFrameData& frame_data);
|
||||||
|
void releaseStreamDecoder_();
|
||||||
|
void releaseStreamDecoderUnlocked_();
|
||||||
|
|
||||||
pa_simple* pulse_simple_ = nullptr; // 修改：PulseAudio 简单 API 句柄
|
pa_simple* pulse_simple_ = nullptr; // 修改：PulseAudio 简单 API 句柄
|
||||||
pa_sample_spec sample_spec_{}; // 新增：PulseAudio 采样规格
|
pa_sample_spec sample_spec_{}; // 新增：PulseAudio 采样规格
|
||||||
@ -51,6 +64,7 @@ namespace cmvr::device {
|
|||||||
std::shared_ptr<std::thread> decode_thread_;
|
std::shared_ptr<std::thread> decode_thread_;
|
||||||
std::shared_ptr<std::thread> play_thread_;
|
std::shared_ptr<std::thread> play_thread_;
|
||||||
std::mutex mtx_;
|
std::mutex mtx_;
|
||||||
|
std::mutex stop_mtx_;
|
||||||
std::string audio_path_;
|
std::string audio_path_;
|
||||||
|
|
||||||
// Boost 单生产者单消费者无锁队列
|
// Boost 单生产者单消费者无锁队列
|
||||||
@ -60,10 +74,20 @@ namespace cmvr::device {
|
|||||||
mutable std::mutex mtx_pause_;
|
mutable std::mutex mtx_pause_;
|
||||||
std::condition_variable cv_pause_;
|
std::condition_variable cv_pause_;
|
||||||
std::atomic<bool> is_paused_{false};
|
std::atomic<bool> is_paused_{false};
|
||||||
|
std::atomic<bool> is_stopping_{false};
|
||||||
|
|
||||||
// 音频时钟同步
|
// 音频时钟同步
|
||||||
std::atomic<int64_t> audio_pts_{0};
|
std::atomic<int64_t> audio_pts_{0};
|
||||||
std::chrono::time_point<std::chrono::steady_clock> playback_start_time_;
|
std::chrono::time_point<std::chrono::steady_clock> playback_start_time_;
|
||||||
|
std::mutex stream_decode_mtx_;
|
||||||
|
AVCodecContext* stream_decoder_ctx_ = nullptr;
|
||||||
|
SwrContext* stream_swr_ctx_ = nullptr;
|
||||||
|
AVFrame* stream_decode_frame_ = nullptr;
|
||||||
|
AVPacket* stream_decode_packet_ = nullptr;
|
||||||
|
std::string stream_codec_;
|
||||||
|
int stream_input_sample_rate_ = 0;
|
||||||
|
int stream_input_channels_ = 0;
|
||||||
|
bool is_streaming_input_ = false;
|
||||||
|
|
||||||
config::FFMpegSpeakerConfig config_;
|
config::FFMpegSpeakerConfig config_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -4,6 +4,9 @@
|
|||||||
//
|
//
|
||||||
|
|
||||||
#include <filesystem>
|
#include <filesystem>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <climits>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
#include "../include/ffmpeg_speaker.h"
|
#include "../include/ffmpeg_speaker.h"
|
||||||
|
|
||||||
@ -18,7 +21,7 @@ ffmpegSpeaker::ffmpegSpeaker(const config::FFMpegSpeakerConfig& cfg):config_(cfg
|
|||||||
try {
|
try {
|
||||||
id_ = config_.id();
|
id_ = config_.id();
|
||||||
memset(&sample_spec_, 0, sizeof(sample_spec_));
|
memset(&sample_spec_, 0, sizeof(sample_spec_));
|
||||||
// state_.volume = config_.volume();
|
state_.volume = 100;
|
||||||
}
|
}
|
||||||
catch (const exception& e) {
|
catch (const exception& e) {
|
||||||
CMVR_LOG(ERROR) << "[ffmpegSpeaker] ([ffmpegSpeaker]): Failed to parse config: " << e.what();
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] ([ffmpegSpeaker]): Failed to parse config: " << e.what();
|
||||||
@ -28,6 +31,7 @@ ffmpegSpeaker::ffmpegSpeaker(const config::FFMpegSpeakerConfig& cfg):config_(cfg
|
|||||||
|
|
||||||
|
|
||||||
ffmpegSpeaker::~ffmpegSpeaker() {
|
ffmpegSpeaker::~ffmpegSpeaker() {
|
||||||
|
is_stopping_ = true;
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(mtx_);
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
state_.is_running = false;
|
state_.is_running = false;
|
||||||
@ -76,6 +80,8 @@ bool ffmpegSpeaker::start() {
|
|||||||
|
|
||||||
void ffmpegSpeaker::resetPlayState()
|
void ffmpegSpeaker::resetPlayState()
|
||||||
{
|
{
|
||||||
|
releaseStreamDecoder_();
|
||||||
|
|
||||||
if (pulse_simple_) {
|
if (pulse_simple_) {
|
||||||
pa_simple_flush(pulse_simple_, nullptr);
|
pa_simple_flush(pulse_simple_, nullptr);
|
||||||
pa_simple_free(pulse_simple_);
|
pa_simple_free(pulse_simple_);
|
||||||
@ -89,19 +95,24 @@ void ffmpegSpeaker::resetPlayState()
|
|||||||
}
|
}
|
||||||
|
|
||||||
state_.is_initialized = false;
|
state_.is_initialized = false;
|
||||||
|
is_streaming_input_ = false;
|
||||||
|
|
||||||
audio_path_.clear();
|
audio_path_.clear();
|
||||||
CMVR_LOG(INFO) << "[ffmpegSpeaker] (resetPlayState): Success, id=" << id_;
|
CMVR_LOG(INFO) << "[ffmpegSpeaker] (resetPlayState): Success, id=" << id_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ffmpegSpeaker::stop() {
|
bool ffmpegSpeaker::stop() {
|
||||||
|
std::lock_guard<std::mutex> stop_lock(stop_mtx_);
|
||||||
|
is_stopping_ = true;
|
||||||
|
|
||||||
{
|
{
|
||||||
lock_guard lock(mtx_);
|
lock_guard lock(mtx_);
|
||||||
state_.is_running = false;
|
state_.is_running = false;
|
||||||
state_.is_decoding = false;
|
state_.is_decoding = false;
|
||||||
state_.is_paused = false;
|
state_.is_paused = false;
|
||||||
|
is_streaming_input_ = false;
|
||||||
}
|
}
|
||||||
|
cv_pause_.notify_all();
|
||||||
// 等待线程结束
|
// 等待线程结束
|
||||||
if (decode_thread_ && decode_thread_->joinable()) {
|
if (decode_thread_ && decode_thread_->joinable()) {
|
||||||
decode_thread_->join();
|
decode_thread_->join();
|
||||||
@ -114,6 +125,7 @@ bool ffmpegSpeaker::stop() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
resetPlayState();
|
resetPlayState();
|
||||||
|
is_stopping_ = false;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -150,6 +162,7 @@ void ffmpegSpeaker::resume() {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
state_.is_paused = false;
|
state_.is_paused = false;
|
||||||
|
cv_pause_.notify_all();
|
||||||
CMVR_LOG(INFO) << "[ffmpegSpeaker] (resume): Success, id=" << id_;
|
CMVR_LOG(INFO) << "[ffmpegSpeaker] (resume): Success, id=" << id_;
|
||||||
}
|
}
|
||||||
catch (const exception& e) {
|
catch (const exception& e) {
|
||||||
@ -531,6 +544,310 @@ void ffmpegSpeaker::play_audio_() {
|
|||||||
CMVR_LOG(INFO) << "[ffmpegSpeaker] Playback finished";
|
CMVR_LOG(INFO) << "[ffmpegSpeaker] Playback finished";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool ffmpegSpeaker::pushAudioFrame(const AudioStreamFrameData& frame_data)
|
||||||
|
{
|
||||||
|
if (is_stopping_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame_data.data.empty()) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool need_start = false;
|
||||||
|
bool need_restart = false;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
need_start = !state_.is_running;
|
||||||
|
need_restart = state_.is_running && is_streaming_input_ &&
|
||||||
|
(frame_data.sample_rate != sample_rate_ || frame_data.channels != channels_);
|
||||||
|
|
||||||
|
if (state_.is_running && !is_streaming_input_) {
|
||||||
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] cannot stream audio while file playback is active";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (need_restart) {
|
||||||
|
stopStreaming();
|
||||||
|
need_start = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (need_start && !startStreamingPlayback_(frame_data)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame_data.format == AudioStreamFormat::PCM ||
|
||||||
|
frame_data.codec == "pcm_s16le" ||
|
||||||
|
frame_data.codec == "pcm") {
|
||||||
|
return pushPcmFrame_(frame_data);
|
||||||
|
}
|
||||||
|
return decodeStreamFrame_(frame_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ffmpegSpeaker::stopStreaming()
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> stop_lock(stop_mtx_);
|
||||||
|
is_stopping_ = true;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
if (!is_streaming_input_) {
|
||||||
|
is_stopping_ = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
state_.is_decoding = false;
|
||||||
|
state_.is_paused = false;
|
||||||
|
is_streaming_input_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv_pause_.notify_all();
|
||||||
|
|
||||||
|
if (play_thread_ && play_thread_->joinable()) {
|
||||||
|
play_thread_->join();
|
||||||
|
play_thread_.reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
resetPlayState();
|
||||||
|
is_stopping_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ffmpegSpeaker::startStreamingPlayback_(const AudioStreamFrameData& frame_data)
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
|
||||||
|
sample_rate_ = frame_data.sample_rate > 0 ? frame_data.sample_rate : 44100;
|
||||||
|
channels_ = frame_data.channels > 0 ? frame_data.channels : 2;
|
||||||
|
|
||||||
|
AudioFrame queued_frame;
|
||||||
|
while (audio_queue_.pop(queued_frame)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!initPulseDevice_()) {
|
||||||
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] PulseAudio device initialization failed";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
state_.is_running = true;
|
||||||
|
state_.is_decoding = true;
|
||||||
|
state_.is_paused = false;
|
||||||
|
is_streaming_input_ = true;
|
||||||
|
|
||||||
|
play_thread_ = std::make_shared<std::thread>([this]() {
|
||||||
|
try {
|
||||||
|
this->play_audio_();
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] Stream play thread exception: " << e.what();
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
state_.is_running = false;
|
||||||
|
state_.is_decoding = false;
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ffmpegSpeaker::pushPcmFrame_(const AudioStreamFrameData& frame_data)
|
||||||
|
{
|
||||||
|
const size_t sample_count = frame_data.data.size() / sizeof(int16_t);
|
||||||
|
if (sample_count == 0) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto buffer = std::make_shared<std::vector<int16_t>>(sample_count);
|
||||||
|
std::memcpy(buffer->data(), frame_data.data.data(), sample_count * sizeof(int16_t));
|
||||||
|
|
||||||
|
float volume_scale = 1.0f;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
volume_scale = static_cast<float>(state_.volume) / 100.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int16_t& sample : *buffer) {
|
||||||
|
const float scaled = static_cast<float>(sample) * volume_scale;
|
||||||
|
sample = static_cast<int16_t>(std::clamp(scaled, -32768.f, 32767.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
if (!state_.is_running || is_stopping_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (audio_queue_.push(buffer)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ffmpegSpeaker::decodeStreamFrame_(const AudioStreamFrameData& frame_data)
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> decode_lock(stream_decode_mtx_);
|
||||||
|
if (!stream_decoder_ctx_ ||
|
||||||
|
stream_codec_ != frame_data.codec ||
|
||||||
|
stream_input_sample_rate_ != frame_data.sample_rate ||
|
||||||
|
stream_input_channels_ != frame_data.channels) {
|
||||||
|
releaseStreamDecoderUnlocked_();
|
||||||
|
if (!initStreamDecoder_(frame_data)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
av_packet_unref(stream_decode_packet_);
|
||||||
|
if (av_new_packet(stream_decode_packet_, static_cast<int>(frame_data.data.size())) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
std::memcpy(stream_decode_packet_->data, frame_data.data.data(), frame_data.data.size());
|
||||||
|
stream_decode_packet_->pts = frame_data.pts;
|
||||||
|
|
||||||
|
int ret = avcodec_send_packet(stream_decoder_ctx_, stream_decode_packet_);
|
||||||
|
av_packet_unref(stream_decode_packet_);
|
||||||
|
if (ret < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
while ((ret = avcodec_receive_frame(stream_decoder_ctx_, stream_decode_frame_)) == 0) {
|
||||||
|
if (!stream_swr_ctx_) {
|
||||||
|
stream_swr_ctx_ = swr_alloc_set_opts(nullptr,
|
||||||
|
av_get_default_channel_layout(channels_),
|
||||||
|
AV_SAMPLE_FMT_S16,
|
||||||
|
sample_rate_,
|
||||||
|
av_get_default_channel_layout(stream_decode_frame_->channels),
|
||||||
|
static_cast<AVSampleFormat>(stream_decode_frame_->format),
|
||||||
|
stream_decode_frame_->sample_rate,
|
||||||
|
0,
|
||||||
|
nullptr);
|
||||||
|
if (!stream_swr_ctx_ || swr_init(stream_swr_ctx_) < 0) {
|
||||||
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] Failed to init stream resampler";
|
||||||
|
av_frame_unref(stream_decode_frame_);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int64_t max_samples64 = av_rescale_rnd(
|
||||||
|
swr_get_delay(stream_swr_ctx_, stream_decode_frame_->sample_rate) + stream_decode_frame_->nb_samples,
|
||||||
|
sample_rate_,
|
||||||
|
stream_decode_frame_->sample_rate,
|
||||||
|
AV_ROUND_UP);
|
||||||
|
const int max_samples = static_cast<int>(std::min<int64_t>(max_samples64, INT_MAX));
|
||||||
|
|
||||||
|
auto buffer = std::make_shared<std::vector<int16_t>>(max_samples * channels_);
|
||||||
|
uint8_t* out[] = {reinterpret_cast<uint8_t*>(buffer->data()), nullptr};
|
||||||
|
const int out_samples = swr_convert(stream_swr_ctx_,
|
||||||
|
out,
|
||||||
|
max_samples,
|
||||||
|
const_cast<const uint8_t**>(stream_decode_frame_->data),
|
||||||
|
stream_decode_frame_->nb_samples);
|
||||||
|
av_frame_unref(stream_decode_frame_);
|
||||||
|
if (out_samples <= 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
buffer->resize(out_samples * channels_);
|
||||||
|
|
||||||
|
float volume_scale = 1.0f;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
volume_scale = static_cast<float>(state_.volume) / 100.0f;
|
||||||
|
}
|
||||||
|
for (int16_t& sample : *buffer) {
|
||||||
|
const float scaled = static_cast<float>(sample) * volume_scale;
|
||||||
|
sample = static_cast<int16_t>(std::clamp(scaled, -32768.f, 32767.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mtx_);
|
||||||
|
if (!state_.is_running || is_stopping_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (audio_queue_.push(buffer)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret == AVERROR(EAGAIN) || ret == AVERROR_EOF;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ffmpegSpeaker::initStreamDecoder_(const AudioStreamFrameData& frame_data)
|
||||||
|
{
|
||||||
|
const AVCodec* decoder = nullptr;
|
||||||
|
if (!frame_data.codec.empty()) {
|
||||||
|
decoder = avcodec_find_decoder_by_name(frame_data.codec.c_str());
|
||||||
|
}
|
||||||
|
if (!decoder) {
|
||||||
|
AVCodecID codec_id = AV_CODEC_ID_NONE;
|
||||||
|
if (frame_data.format == AudioStreamFormat::AAC) {
|
||||||
|
codec_id = AV_CODEC_ID_AAC;
|
||||||
|
} else if (frame_data.format == AudioStreamFormat::MP3) {
|
||||||
|
codec_id = AV_CODEC_ID_MP3;
|
||||||
|
}
|
||||||
|
if (codec_id != AV_CODEC_ID_NONE) {
|
||||||
|
decoder = avcodec_find_decoder(codec_id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!decoder) {
|
||||||
|
CMVR_LOG(ERROR) << "[ffmpegSpeaker] Unsupported stream audio codec: " << frame_data.codec;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
stream_decoder_ctx_ = avcodec_alloc_context3(decoder);
|
||||||
|
if (!stream_decoder_ctx_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
stream_decoder_ctx_->sample_rate = frame_data.sample_rate > 0 ? frame_data.sample_rate : sample_rate_;
|
||||||
|
stream_decoder_ctx_->channels = frame_data.channels > 0 ? frame_data.channels : channels_;
|
||||||
|
stream_decoder_ctx_->channel_layout =
|
||||||
|
stream_decoder_ctx_->channels == 1 ? AV_CH_LAYOUT_MONO : AV_CH_LAYOUT_STEREO;
|
||||||
|
|
||||||
|
if (avcodec_open2(stream_decoder_ctx_, decoder, nullptr) < 0) {
|
||||||
|
releaseStreamDecoderUnlocked_();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
stream_decode_frame_ = av_frame_alloc();
|
||||||
|
stream_decode_packet_ = av_packet_alloc();
|
||||||
|
if (!stream_decode_frame_ || !stream_decode_packet_) {
|
||||||
|
releaseStreamDecoderUnlocked_();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
stream_codec_ = frame_data.codec;
|
||||||
|
stream_input_sample_rate_ = frame_data.sample_rate;
|
||||||
|
stream_input_channels_ = frame_data.channels;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ffmpegSpeaker::releaseStreamDecoder_()
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> decode_lock(stream_decode_mtx_);
|
||||||
|
releaseStreamDecoderUnlocked_();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ffmpegSpeaker::releaseStreamDecoderUnlocked_()
|
||||||
|
{
|
||||||
|
if (stream_swr_ctx_) {
|
||||||
|
swr_free(&stream_swr_ctx_);
|
||||||
|
}
|
||||||
|
if (stream_decode_frame_) {
|
||||||
|
av_frame_free(&stream_decode_frame_);
|
||||||
|
}
|
||||||
|
if (stream_decode_packet_) {
|
||||||
|
av_packet_free(&stream_decode_packet_);
|
||||||
|
}
|
||||||
|
if (stream_decoder_ctx_) {
|
||||||
|
avcodec_free_context(&stream_decoder_ctx_);
|
||||||
|
}
|
||||||
|
stream_codec_.clear();
|
||||||
|
stream_input_sample_rate_ = 0;
|
||||||
|
stream_input_channels_ = 0;
|
||||||
|
}
|
||||||
|
|
||||||
bool ffmpegSpeaker::initPulseDevice_() {
|
bool ffmpegSpeaker::initPulseDevice_() {
|
||||||
// 验证参数
|
// 验证参数
|
||||||
if (sample_rate_ <= 0 || channels_ <= 0) {
|
if (sample_rate_ <= 0 || channels_ <= 0) {
|
||||||
|
|||||||
@ -6,12 +6,14 @@
|
|||||||
#define CMVR_ES_STATE_DEFINE_H
|
#define CMVR_ES_STATE_DEFINE_H
|
||||||
|
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
#include <cstdint>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <vector>
|
#include <string>
|
||||||
#include <unordered_map>
|
|
||||||
#include <set>
|
#include <set>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/types/agv/agv_types.h"
|
||||||
#include "common/types/geometry_types.h"
|
#include "common/types/geometry_types.h"
|
||||||
|
|
||||||
|
|
||||||
@ -34,10 +36,35 @@ namespace cmvr::device{
|
|||||||
UNKNOWN
|
UNKNOWN
|
||||||
};
|
};
|
||||||
|
|
||||||
// ------------------------------------- AGV -------------------------------------
|
enum class AudioStreamFormat {
|
||||||
typedef struct{
|
PCM = 0,
|
||||||
|
MP3 = 1,
|
||||||
|
AAC = 2,
|
||||||
|
WAV = 3,
|
||||||
|
OPUS = 4,
|
||||||
|
UNKNOWN = 99
|
||||||
|
};
|
||||||
|
|
||||||
} AGVState;
|
struct AudioStreamFrameData {
|
||||||
|
std::vector<uint8_t> data;
|
||||||
|
int sample_rate = 44100;
|
||||||
|
int channels = 2;
|
||||||
|
AudioStreamFormat format = AudioStreamFormat::PCM;
|
||||||
|
std::string codec = "pcm_s16le";
|
||||||
|
int64_t pts = 0;
|
||||||
|
int64_t dts = 0;
|
||||||
|
int nb_samples = 0;
|
||||||
|
uint64_t stream_epoch = 0;
|
||||||
|
uint64_t sequence = 0;
|
||||||
|
int64_t capture_monotonic_ns = 0;
|
||||||
|
int64_t capture_utc_ns = 0;
|
||||||
|
int32_t time_base_num = 1;
|
||||||
|
int32_t time_base_den = 44100;
|
||||||
|
int64_t duration = 0;
|
||||||
|
bool discontinuity = false;
|
||||||
|
uint32_t codec_config_generation = 0;
|
||||||
|
std::vector<uint8_t> codec_config;
|
||||||
|
};
|
||||||
|
|
||||||
// ------------------------------------- robot -------------------------------------
|
// ------------------------------------- robot -------------------------------------
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|||||||
89
cmvr-es/hardware/README.md
Normal file
89
cmvr-es/hardware/README.md
Normal file
@ -0,0 +1,89 @@
|
|||||||
|
# Hardware 模块开发指南
|
||||||
|
|
||||||
|
`hardware/` 提供串口等可复用的底层传输能力。设备命令、寄存器含义和厂商协议应保留在 `devices/` 的具体后端中。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 当前组件
|
||||||
|
|
||||||
|
| 文件 | 作用 |
|
||||||
|
| --- | --- |
|
||||||
|
| [`abstract_serial_transport.h`](include/abstract_serial_transport.h) | 可替换、可测试的串口传输接口 |
|
||||||
|
| [`posix_serial_transport.h`](include/posix_serial_transport.h) | Linux POSIX 串口实现，支持超时和任意正波特率 |
|
||||||
|
| [`serial_interface.h`](include/serial_interface.h) | 现有 RS485/CAN 寄存器辅助接口 |
|
||||||
|
| [`esp32_serial_port.h`](include/esp32_serial_port.h) | ESP32/舵机串口辅助实现 |
|
||||||
|
|
||||||
|
新设备优先依赖 `AbstractSerialTransport`，通过构造或 setter 注入实现。这样单元测试可以使用 fake transport，不需要真实 `/dev/tty*`。
|
||||||
|
|
||||||
|
## 底层与设备层边界
|
||||||
|
|
||||||
|
适合放在 `hardware/`：
|
||||||
|
|
||||||
|
- 打开、关闭和查询传输状态；
|
||||||
|
- 有限超时的字节读写；
|
||||||
|
- 输入缓冲清理；
|
||||||
|
- OS 错误转换和统一错误文本；
|
||||||
|
- 与设备语义无关的帧读写工具。
|
||||||
|
|
||||||
|
应放在 `devices/<category>/<backend>/`：
|
||||||
|
|
||||||
|
- 帧头、命令码、寄存器地址和校验规则；
|
||||||
|
- 重试、握手和设备状态机；
|
||||||
|
- 设备 ID、关节、传感器和错误码含义；
|
||||||
|
- 后台接收线程及其与设备状态的同步。
|
||||||
|
|
||||||
|
## 新增传输实现
|
||||||
|
|
||||||
|
1. 实现 `AbstractSerialTransport` 或为新总线定义同样窄的抽象；
|
||||||
|
2. 析构函数必须可靠释放 fd、handle 和后台线程；
|
||||||
|
3. `open()` 失败后对象保持可再次打开；
|
||||||
|
4. `close()` 应幂等；
|
||||||
|
5. 所有阻塞读取必须支持有限 timeout；
|
||||||
|
6. `lastError()` 返回最近一次操作的可诊断错误；
|
||||||
|
7. 更新 [`CMakeLists.txt`](CMakeLists.txt) 和 `cmvr_es::hardware` target；
|
||||||
|
8. 使用 pseudo terminal、socketpair 或 fake 实现编写无设备测试。
|
||||||
|
|
||||||
|
## 并发与事务
|
||||||
|
|
||||||
|
`PosixSerialTransport` 使用互斥锁保护单次 `open/close/read/write`。这不等于一个“write command + read response”复合事务天然不会与其他线程交错。
|
||||||
|
|
||||||
|
具体驱动必须：
|
||||||
|
|
||||||
|
- 由一个 I/O 线程独占 transport，或在驱动层为完整请求/响应加锁；
|
||||||
|
- 在关闭前先通知并 join 接收线程；
|
||||||
|
- 不持有设备状态锁执行长时间 I/O；
|
||||||
|
- 超时后清理残留输入，避免下一请求读到旧响应；
|
||||||
|
- 不让回调在 transport 锁内反向调用设备；
|
||||||
|
- 为部分写、短读、EINTR、EAGAIN 和设备拔出定义行为。
|
||||||
|
|
||||||
|
## 权限和部署
|
||||||
|
|
||||||
|
Linux 设备通常需要：
|
||||||
|
|
||||||
|
- 将运行用户加入 `dialout`、`video` 或设备专用组；
|
||||||
|
- 使用 udev rule 固定权限和稳定设备别名；
|
||||||
|
- CAN 接口在服务启动前完成 `ip link` 配置；
|
||||||
|
- 不依赖开发机上的临时 `chmod 777`；
|
||||||
|
- 在日志中记录逻辑设备名，不输出密码和完整敏感报文。
|
||||||
|
|
||||||
|
## 新增总线类型
|
||||||
|
|
||||||
|
新增 CAN、TCP、USB 等通用传输时：
|
||||||
|
|
||||||
|
1. 定义只描述字节/帧传输的抽象接口；
|
||||||
|
2. 将 Linux/SDK 实现与抽象分离；
|
||||||
|
3. 让设备后端依赖抽象而不是全局单例；
|
||||||
|
4. 提供 fake 实现和故障注入；
|
||||||
|
5. 记录线程安全级别和调用顺序；
|
||||||
|
6. 更新安装依赖及 systemd 权限说明。
|
||||||
|
|
||||||
|
## 测试清单
|
||||||
|
|
||||||
|
- [ ] 打开、重复打开、关闭和重复关闭
|
||||||
|
- [ ] 非法路径和非法参数
|
||||||
|
- [ ] 完整写、部分写和对端断开
|
||||||
|
- [ ] 精确长度读取、短读和超时
|
||||||
|
- [ ] EINTR/EAGAIN 重试
|
||||||
|
- [ ] I/O 期间关闭
|
||||||
|
- [ ] 多线程调用或显式拒绝并发
|
||||||
|
- [ ] 析构后没有 fd 和线程泄漏
|
||||||
252
cmvr-es/manager/README.md
Normal file
252
cmvr-es/manager/README.md
Normal file
@ -0,0 +1,252 @@
|
|||||||
|
# Manager 模块开发指南
|
||||||
|
|
||||||
|
`manager/` 负责组织设备、任务和协议无关媒体源。Manager 管理对象生命周期和调度，不实现厂商协议，也不实现平台 wire protocol。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 当前管理器
|
||||||
|
|
||||||
|
| 目录 | CMake target | 职责 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| [`device_manager/`](device_manager/) | `cmvr_es::device_manager` | 按配置创建、初始化、查询和批量启停设备 |
|
||||||
|
| [`task_manager/`](task_manager/) | `cmvr_es::task_manager` | 创建任务、校验运行模式、统一启停和调度周期任务 |
|
||||||
|
| [`media_source_hub/`](media_source_hub/) | `cmvr_es::media_source_hub`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 |
|
||||||
|
|
||||||
|
`manager/` 当前没有聚合 `CMakeLists.txt`，三个子目录由 [`../CMakeLists.txt`](../CMakeLists.txt) 分别加入。新增 manager 时必须显式更新该文件。
|
||||||
|
|
||||||
|
## 进程生命周期
|
||||||
|
|
||||||
|
当前 [`../main.cpp`](../main.cpp) 的顺序是：
|
||||||
|
|
||||||
|
1. 加载根配置并设置全局配置根；
|
||||||
|
2. 构造 `DeviceManager`；
|
||||||
|
3. 创建启用的设备并调用 `device->init()`；
|
||||||
|
4. 注册 gRPC、QUIC TaskFactory creator；
|
||||||
|
5. 构造 `TaskManager` 并调用启用任务的 `init()`；
|
||||||
|
6. `startRunTask()` 启动任务和周期调度线程；
|
||||||
|
7. 收到 SIGINT/SIGTERM 后调用 `TaskManager::stopRunTask()`。
|
||||||
|
|
||||||
|
重要限制：
|
||||||
|
|
||||||
|
- DeviceManager 构造不会自动调用全部设备的 `start()`；
|
||||||
|
- 当前主退出路径没有调用 `DeviceManager::stop()`；
|
||||||
|
- `SystemService/StopAll` 会调用 DeviceManager stop；
|
||||||
|
- `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止；
|
||||||
|
- `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回；
|
||||||
|
- DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。
|
||||||
|
|
||||||
|
## DeviceManager
|
||||||
|
|
||||||
|
关键文件：
|
||||||
|
|
||||||
|
- [`device_manager/include/device_manager.h`](device_manager/include/device_manager.h)
|
||||||
|
- [`device_manager/include/device_factory.h`](device_manager/include/device_factory.h)
|
||||||
|
- [`device_manager/src/device_factory.cpp`](device_manager/src/device_factory.cpp)
|
||||||
|
- [`../devices/README.md`](../devices/README.md)
|
||||||
|
|
||||||
|
### 增加现有类别的新后端
|
||||||
|
|
||||||
|
例如增加一种摄像头：
|
||||||
|
|
||||||
|
1. 扩展类别配置 Proto 的 `oneof backend`；
|
||||||
|
2. 实现对应抽象设备；
|
||||||
|
3. 修改 `CameraFactory::create()`；
|
||||||
|
4. 增加 CMake target；
|
||||||
|
5. 在摄像头集合配置中增加实例；
|
||||||
|
6. 在 DeviceManager 配置中增加相同 ID 的条目。
|
||||||
|
|
||||||
|
这种扩展通常不修改全局 `DeviceFactory`，因为 `DEVICE_TYPE_CAMERA` 已经路由到 CameraFactory。
|
||||||
|
|
||||||
|
### 增加全新设备类别
|
||||||
|
|
||||||
|
还需要：
|
||||||
|
|
||||||
|
1. 扩展 `DeviceConfigEntry::DeviceType`；
|
||||||
|
2. 扩展 `DeviceKind` 和字符串映射；
|
||||||
|
3. 在 `DeviceFactory::DeviceFactory()` 注册 creator；
|
||||||
|
4. 扩展 DeviceManager 的设备类型日志映射；
|
||||||
|
5. 为 `getDevice<NewAbstractType>()` 增加显式模板实例化；
|
||||||
|
6. 在 device_manager target 链接新设备 target；
|
||||||
|
7. 如需平台访问，增加 API Proto、gRPC service 和系统设备类型映射。
|
||||||
|
|
||||||
|
`DeviceFactory::registerCreator()` 虽然是 public，但 factory 是 DeviceManager 的私有成员，当前不是运行时插件入口。新增全局设备类别仍需修改 `device_factory.cpp`。
|
||||||
|
|
||||||
|
### 容器和顺序约束
|
||||||
|
|
||||||
|
- 配置启用条目按配置顺序创建并 `init()`；
|
||||||
|
- 有初始化依赖的设备应把依赖项写在使用方之前；
|
||||||
|
- `start()`、`stop()` 遍历 `unordered_map`，不能依赖启停顺序；
|
||||||
|
- 某个设备 start 返回 false 时，当前实现会继续启动其他设备且不会回滚；
|
||||||
|
- DeviceManager 会把 create/init/start/stop 和健康探针异常转换成设备状态错误，
|
||||||
|
但后端仍应把预期失败转换为返回值；
|
||||||
|
- collection 配置要求 manager entry ID 能找到同 ID 子配置；
|
||||||
|
- ID 重复、不匹配或配置路径为空都会拒绝创建；
|
||||||
|
- `registerDevice()` 不会替调用方调用 `init()`；
|
||||||
|
- `devices_` 和状态表由读写锁保护，运行期动态注册不会与 heartbeat/query
|
||||||
|
形成数据竞争；但动态设备不会自动补执行已经发生的 `start()`，也暂不支持移除；
|
||||||
|
- `getDevice<T>()` 类型不匹配或 ID 不存在时返回空指针。
|
||||||
|
|
||||||
|
### 状态快照与 QUIC Heartbeat
|
||||||
|
|
||||||
|
`DeviceManager::snapshot()` 返回协议无关的纯值快照，包含 Manager
|
||||||
|
名称、版本、描述以及按设备 ID 排序的完整设备表。状态表与可用设备对象表分开：
|
||||||
|
|
||||||
|
- 禁用、创建失败、初始化失败的配置项仍会出现在快照中；
|
||||||
|
- `devices_` 仍只保存可供业务查询的已初始化对象，不改变现有 service 语义；
|
||||||
|
- 动态注册设备初始为 `REGISTERED`，不会冒充已经由 Manager 初始化或启动；
|
||||||
|
- create/init/start/stop 的已知错误会设置 Manager `ERROR` 和 `has_error`；设备
|
||||||
|
health 与生命周期独立，仍由探针报告 `HEALTHY/DEGRADED/FAULT/UNKNOWN`；
|
||||||
|
- `healthSnapshot()` 未实现时为 `UNKNOWN`，不能解释为健康；
|
||||||
|
- heartbeat 只消费该内存快照，并在 QUIC 映射边界过滤 `enabled=false` 的设备；
|
||||||
|
禁止从发送线程同步访问厂商 SDK 或设备网络。
|
||||||
|
|
||||||
|
快照复制设备元数据和临时 `shared_ptr` 后立即释放容器锁，再调用设备的轻量健康
|
||||||
|
探针，避免持锁进入设备代码。start/stop 同样在锁外调用设备，并由单独的生命周期
|
||||||
|
锁防止同一 Manager 并发启停。
|
||||||
|
|
||||||
|
不要在仍有 service/task 持有 manager 引用时调用 `destroyInstance()`。
|
||||||
|
|
||||||
|
## TaskManager
|
||||||
|
|
||||||
|
完整任务实现指南见 [`../task/README.md`](../task/README.md)。
|
||||||
|
|
||||||
|
### PERIODIC_STEP
|
||||||
|
|
||||||
|
- `control_period_s` 必须是有限正数；
|
||||||
|
- 所有周期任务的 `step()` 串行运行在同一个 scheduler 线程；
|
||||||
|
- 慢 I/O 或长计算会延迟其他周期任务；
|
||||||
|
- 只有 state 为 `RUNNING` 的任务会执行 `step()`；
|
||||||
|
- `step()` 内不能同步调用 `stopRunTask()` 等待当前 scheduler 自身。
|
||||||
|
|
||||||
|
### BLOCKING_SERVICE
|
||||||
|
|
||||||
|
- TaskManager 只调用 `init/start/stop`，不调用 `step()`；
|
||||||
|
- `start()` 仍必须快速返回，由任务自己持有服务线程或事件循环；
|
||||||
|
- `stop()` 必须唤醒阻塞操作并 join 自己创建的线程。
|
||||||
|
|
||||||
|
### 启停语义
|
||||||
|
|
||||||
|
- manager 构造阶段只调用 task `init()`；
|
||||||
|
- 任一 task `start()` 失败，会停止此前已启动的任务且不启动 scheduler；
|
||||||
|
- 返回 false 的 task 必须自行清理本次 start 已经创建的部分资源，TaskManager 不会再调用该失败 task 的 stop；
|
||||||
|
- TaskManager 处于 running 状态时，停止流程先停止并 join scheduler，再调用每个 task 的 `stop()`；
|
||||||
|
- 未启动或已经停止时，`stopRunTask()` 会直接返回，不会再次逐个调用 task stop；
|
||||||
|
- 任务存储在 `unordered_map`，启动和停止顺序不确定；
|
||||||
|
- 有顺序依赖的工作应放入同一协调任务或显式建模；
|
||||||
|
- task 返回后，其内部状态并发安全由具体实现负责。
|
||||||
|
|
||||||
|
## MediaSourceHub
|
||||||
|
|
||||||
|
关键文件：
|
||||||
|
|
||||||
|
- [`media_source_hub/include/media_source_hub.h`](media_source_hub/include/media_source_hub.h)
|
||||||
|
- [`media_source_hub/src/device_media_source_adapter.cpp`](media_source_hub/src/device_media_source_adapter.cpp)
|
||||||
|
- [`../common/media/media_frame.h`](../common/media/media_frame.h)
|
||||||
|
- [`../common/base/ring_buffer.h`](../common/base/ring_buffer.h)
|
||||||
|
|
||||||
|
当前默认轨道：
|
||||||
|
|
||||||
|
| 来源 | Track ID | 默认 ring capacity |
|
||||||
|
| --- | --- | ---: |
|
||||||
|
| 摄像头彩色流 | `<device_id>/video/color` | 64 |
|
||||||
|
| 麦克风主流 | `<device_id>/audio/main` | 256 |
|
||||||
|
|
||||||
|
当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 Hub；gRPC Depth/RGBD 仍直接读取设备帧。
|
||||||
|
|
||||||
|
### 注册新媒体源
|
||||||
|
|
||||||
|
1. 创建不可变 `TrackDescriptor`；
|
||||||
|
2. 使用唯一且稳定的 `track_id`；
|
||||||
|
3. 提供 `start(sink, cancelled)`；
|
||||||
|
4. 提供同步 `stop()`；
|
||||||
|
5. 视频源按需提供 `request_key_frame()`；
|
||||||
|
6. 调用 `registerSource()`；
|
||||||
|
7. 生产不可变 `MediaFramePtr`；
|
||||||
|
8. 保证 frame descriptor ID 与注册轨道一致；
|
||||||
|
9. 在重启、编码变化和中断时更新 producer 元数据和 discontinuity，由 adapter 更新 descriptor。
|
||||||
|
|
||||||
|
Descriptor 最低要求：
|
||||||
|
|
||||||
|
- `id`、`source_id` 非空；
|
||||||
|
- kind 不能是 `UNKNOWN`；
|
||||||
|
- time base 分子和分母均大于零；
|
||||||
|
- generation 大于零。
|
||||||
|
|
||||||
|
### Source callback 生命周期
|
||||||
|
|
||||||
|
- 第一位 subscriber 触发一次 source start；
|
||||||
|
- 多位 subscriber 共享同一个采集生产者；
|
||||||
|
- 最后一份 Subscription reset/析构时同步调用 source stop；
|
||||||
|
- Hub 会先 close ring 并唤醒消费者；stop callback 必须解除生产端阻塞、停止采集并 join producer，形成同步发布屏障；
|
||||||
|
- start 必须在阻塞阶段检查 cancellation；
|
||||||
|
- cancellation predicate 必须快速、非阻塞，不能回调同一个 Hub；
|
||||||
|
- active source 不能 unregister，应先销毁全部 subscriptions；
|
||||||
|
- request-key-frame 与 stop 串行化，只在 source 运行时调用；
|
||||||
|
- adapter 的最后 lease 只停止媒体 streaming，不等于设备级 stop。
|
||||||
|
|
||||||
|
永久不响应 cancellation 的 start 虽会被隔离以避免 use-after-free，仍可能泄漏线程和外部资源，不能依赖该隔离替代正确实现。
|
||||||
|
|
||||||
|
### Subscription 与广播环形队列
|
||||||
|
|
||||||
|
- 每个消费线程单独调用一次 `subscribe()`；
|
||||||
|
- Subscription move-only 且为单消费者对象；
|
||||||
|
- 同一 Subscription 不能跨线程并发 read/reset；
|
||||||
|
- 满队列覆盖最旧帧，不阻塞生产者；
|
||||||
|
- `dropped_since_last_read` 是该消费者实际错过的帧数；
|
||||||
|
- ring 全局 dropped count 与 cursor dropped count 含义不同；
|
||||||
|
- `NEXT_PUBLISHED` 忽略已有帧；
|
||||||
|
- `OLDEST_AVAILABLE` 从最旧保留帧开始；
|
||||||
|
- `LATEST_AVAILABLE` 读取当前最新帧；
|
||||||
|
- `discardPendingIfExceeds(limit)` 在积压超过阈值时原子地把该消费者游标推进到
|
||||||
|
当前发布末尾，并把主动丢弃数量计入下一次读取的 `dropped_since_last_read`；
|
||||||
|
- `discardPendingIfExceeds()` 与 `read()` 一样只能由该 Subscription 的单一消费
|
||||||
|
线程调用，不能用它替代 Subscription 的线程所有权约束；
|
||||||
|
- source 重启会 reset ring、提升 `BroadcastFrameRing` 内部 generation，并把 ring 的 `ReadResult.sequence` 从 0 重新计数；
|
||||||
|
- close 唤醒等待者并拒绝新 publish。
|
||||||
|
|
||||||
|
ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.sequence` 也不等于 `MediaFrame::sequence`。Descriptor generation 和媒体帧 sequence 仍由 producer/adapter 维护。
|
||||||
|
|
||||||
|
协议消费者需要分别处理 ring `generation_changed`、descriptor generation、消费者 drop 和 frame discontinuity；任一不连续发生时都应传播状态，帧间编码还应请求关键帧。主动丢弃 H.264/H.265 积压后不得直接发送 P/B 帧，必须等新的关键帧恢复。
|
||||||
|
|
||||||
|
## 新增第四种 Manager
|
||||||
|
|
||||||
|
1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceHub 的子职责；
|
||||||
|
2. 定义所有权、初始化、start/stop 和线程模型；
|
||||||
|
3. 避免新增无必要的全局单例；
|
||||||
|
4. 新建独立目录、头文件、实现和 CMake target；
|
||||||
|
5. 在 [`../CMakeLists.txt`](../CMakeLists.txt) 增加子目录；
|
||||||
|
6. 只在 `main.cpp` 或明确的上层 owner 组装；
|
||||||
|
7. 增加无设备生命周期、失败回滚和并发测试。
|
||||||
|
|
||||||
|
## 测试
|
||||||
|
|
||||||
|
MediaSourceHub：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cmake --build build --target media_source_hub_test
|
||||||
|
ctest \
|
||||||
|
--test-dir build \
|
||||||
|
-R '^media_source_hub_test$' \
|
||||||
|
--output-on-failure
|
||||||
|
```
|
||||||
|
|
||||||
|
DeviceManager 已有 `device_manager_snapshot_test`，覆盖全量状态表、生命周期失败、
|
||||||
|
异常限长、排序和值快照并发读取。TaskManager 仍缺少独立 CTest。修改其行为时
|
||||||
|
至少补充：
|
||||||
|
|
||||||
|
- fake device 创建、ID 冲突和 init/start/stop 失败；
|
||||||
|
- 设备依赖顺序；
|
||||||
|
- 周期任务调度和慢 step；
|
||||||
|
- service task 启停、重复 stop 和启动失败回滚；
|
||||||
|
- 并发查询、取消和 shutdown。
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] manager 没有包含厂商 wire protocol
|
||||||
|
- [ ] 初始化、start、stop 和 destroy 语义明确
|
||||||
|
- [ ] 不依赖 unordered_map 的遍历顺序
|
||||||
|
- [ ] 动态集合修改不会与查询并发
|
||||||
|
- [ ] Media Subscription 每线程独立
|
||||||
|
- [ ] stop 能唤醒并 join 所有工作线程
|
||||||
|
- [ ] 新 manager 已加入 `cmvr-es/CMakeLists.txt`
|
||||||
|
- [ ] 无设备失败路径有测试
|
||||||
@ -22,3 +22,29 @@ target_link_libraries(device_manager PRIVATE
|
|||||||
|
|
||||||
add_library(cmvr_es::device_manager ALIAS device_manager)
|
add_library(cmvr_es::device_manager ALIAS device_manager)
|
||||||
install(TARGETS device_manager LIBRARY DESTINATION lib)
|
install(TARGETS device_manager LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
add_executable(device_manager_snapshot_test
|
||||||
|
tests/device_manager_snapshot_test.cpp
|
||||||
|
)
|
||||||
|
target_link_libraries(device_manager_snapshot_test PRIVATE
|
||||||
|
cmvr_es::device_manager
|
||||||
|
)
|
||||||
|
add_test(
|
||||||
|
NAME device_manager_snapshot_test
|
||||||
|
COMMAND device_manager_snapshot_test
|
||||||
|
)
|
||||||
|
set_tests_properties(device_manager_snapshot_test PROPERTIES TIMEOUT 20)
|
||||||
|
if(UNIX AND NOT APPLE)
|
||||||
|
get_property(_device_manager_test_library_dirs
|
||||||
|
DIRECTORY PROPERTY LINK_DIRECTORIES)
|
||||||
|
list(PREPEND _device_manager_test_library_dirs
|
||||||
|
"${CMAKE_BINARY_DIR}/cmvr_compiler_runtime")
|
||||||
|
list(JOIN _device_manager_test_library_dirs ":"
|
||||||
|
_device_manager_test_library_path)
|
||||||
|
set_tests_properties(device_manager_snapshot_test PROPERTIES
|
||||||
|
ENVIRONMENT
|
||||||
|
"LD_LIBRARY_PATH=${_device_manager_test_library_path}"
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
|
|||||||
@ -7,9 +7,12 @@
|
|||||||
|
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
|
#include <shared_mutex>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <unordered_set>
|
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
#include <unordered_set>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
#include "device_factory.h"
|
#include "device_factory.h"
|
||||||
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
|
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
|
||||||
|
|
||||||
@ -31,6 +34,8 @@ namespace cmvr::device {
|
|||||||
void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
|
void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
|
||||||
void registerDevice(const std::shared_ptr<AbstractDevice>& device);
|
void registerDevice(const std::shared_ptr<AbstractDevice>& device);
|
||||||
void registerDevice(const std::string& device_id, const std::shared_ptr<AbstractDevice>& device);
|
void registerDevice(const std::string& device_id, const std::shared_ptr<AbstractDevice>& device);
|
||||||
|
std::shared_ptr<AbstractDevice> getDeviceBase(const std::string& device_id);
|
||||||
|
DeviceManagerSnapshot snapshot() const;
|
||||||
|
|
||||||
std::string version() const;
|
std::string version() const;
|
||||||
std::string name() const;
|
std::string name() const;
|
||||||
@ -44,7 +49,10 @@ namespace cmvr::device {
|
|||||||
static std::shared_ptr<DeviceManager> instance_;
|
static std::shared_ptr<DeviceManager> instance_;
|
||||||
|
|
||||||
config::DeviceManagerConfig cfg_;
|
config::DeviceManagerConfig cfg_;
|
||||||
|
mutable std::shared_mutex devices_mutex_;
|
||||||
|
std::mutex lifecycle_mutex_;
|
||||||
std::unordered_map<std::string, DeviceRecord> devices_;
|
std::unordered_map<std::string, DeviceRecord> devices_;
|
||||||
|
std::unordered_map<std::string, ManagedDeviceSnapshot> device_statuses_;
|
||||||
std::unique_ptr<DeviceFactory> dev_factory_;
|
std::unique_ptr<DeviceFactory> dev_factory_;
|
||||||
|
|
||||||
explicit DeviceManager(const config::DeviceManagerConfig &cfg);
|
explicit DeviceManager(const config::DeviceManagerConfig &cfg);
|
||||||
@ -52,6 +60,8 @@ namespace cmvr::device {
|
|||||||
void pre_scan_robot_arm_dependencies_() const;
|
void pre_scan_robot_arm_dependencies_() const;
|
||||||
void init_devices_();
|
void init_devices_();
|
||||||
void configure_mujoco_viewer_pip_();
|
void configure_mujoco_viewer_pip_();
|
||||||
|
void start_devices_();
|
||||||
|
void stop_devices_();
|
||||||
};
|
};
|
||||||
} // cmvr
|
} // cmvr
|
||||||
|
|
||||||
|
|||||||
@ -5,6 +5,9 @@
|
|||||||
|
|
||||||
#include "../include/device_manager.h"
|
#include "../include/device_manager.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <exception>
|
||||||
|
|
||||||
#include "devices/agv/abstract_agv.h"
|
#include "devices/agv/abstract_agv.h"
|
||||||
#include "devices/arm/robot_arm.h"
|
#include "devices/arm/robot_arm.h"
|
||||||
#include "devices/battery/abstract_battery.h"
|
#include "devices/battery/abstract_battery.h"
|
||||||
@ -203,6 +206,17 @@ std::shared_ptr<DeviceType> DeviceManager::getDevice(const std::string& device_i
|
|||||||
return ptr;
|
return ptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<AbstractDevice> DeviceManager::getDeviceBase(const std::string& device_id)
|
||||||
|
{
|
||||||
|
std::shared_lock lock(devices_mutex_);
|
||||||
|
const auto it = devices_.find(device_id);
|
||||||
|
if (it == devices_.end() || !it->second.device) {
|
||||||
|
CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found.";
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
return it->second.device;
|
||||||
|
}
|
||||||
|
|
||||||
void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>& device_list){
|
void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>& device_list){
|
||||||
device_list.clear();
|
device_list.clear();
|
||||||
for (const auto& [device_id, record] : devices_) {
|
for (const auto& [device_id, record] : devices_) {
|
||||||
@ -246,6 +260,63 @@ void DeviceManager::registerDevice(const std::string& device_id,
|
|||||||
<< ", kind=" << toString(device->kind());
|
<< ", kind=" << toString(device->kind());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
DeviceManagerSnapshot DeviceManager::snapshot() const
|
||||||
|
{
|
||||||
|
struct SnapshotSource {
|
||||||
|
ManagedDeviceSnapshot status;
|
||||||
|
std::shared_ptr<AbstractDevice> device;
|
||||||
|
};
|
||||||
|
|
||||||
|
std::vector<SnapshotSource> sources;
|
||||||
|
{
|
||||||
|
std::shared_lock lock(devices_mutex_);
|
||||||
|
sources.reserve(devices_.size());
|
||||||
|
for (const auto& [id, record] : devices_) {
|
||||||
|
SnapshotSource source;
|
||||||
|
source.status.id = id;
|
||||||
|
source.status.kind = record.kind;
|
||||||
|
source.status.type_name = record.type_name;
|
||||||
|
source.status.enabled = true;
|
||||||
|
source.status.state = ManagedDeviceState::Ready;
|
||||||
|
source.device = record.device;
|
||||||
|
sources.push_back(std::move(source));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
DeviceManagerSnapshot result;
|
||||||
|
result.name = name();
|
||||||
|
result.version = version();
|
||||||
|
result.description = description();
|
||||||
|
result.devices.reserve(sources.size());
|
||||||
|
|
||||||
|
for (auto& source : sources) {
|
||||||
|
if (source.device) {
|
||||||
|
try {
|
||||||
|
source.status.health = source.device->healthSnapshot();
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
source.status.health.state = DeviceHealthState::Fault;
|
||||||
|
source.status.health.error_message = error.what();
|
||||||
|
} catch (...) {
|
||||||
|
source.status.health.state = DeviceHealthState::Fault;
|
||||||
|
source.status.health.error_message =
|
||||||
|
"device health snapshot threw an unknown exception";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
source.status.abnormal =
|
||||||
|
source.status.health.state == DeviceHealthState::Degraded ||
|
||||||
|
source.status.health.state == DeviceHealthState::Fault;
|
||||||
|
source.status.error_message = source.status.health.error_message;
|
||||||
|
result.devices.push_back(std::move(source.status));
|
||||||
|
}
|
||||||
|
|
||||||
|
std::sort(result.devices.begin(), result.devices.end(),
|
||||||
|
[](const ManagedDeviceSnapshot& lhs,
|
||||||
|
const ManagedDeviceSnapshot& rhs) {
|
||||||
|
return lhs.id < rhs.id;
|
||||||
|
});
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
std::string DeviceManager::version() const {
|
std::string DeviceManager::version() const {
|
||||||
return cfg_.version().empty() ? "1.0" : cfg_.version();
|
return cfg_.version().empty() ? "1.0" : cfg_.version();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -0,0 +1,379 @@
|
|||||||
|
#include "manager/device_manager/include/device_manager.h"
|
||||||
|
|
||||||
|
#include "devices/camera/abstract_camera.h"
|
||||||
|
#include "devices/dexhand/abstract_dexhand.h"
|
||||||
|
#include "devices/microphone/abstract_microphone.h"
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <cstddef>
|
||||||
|
#include <memory>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
#define CHECK_TRUE(condition) \
|
||||||
|
do { \
|
||||||
|
if (!(condition)) { \
|
||||||
|
return false; \
|
||||||
|
} \
|
||||||
|
} while (false)
|
||||||
|
|
||||||
|
using cmvr::device::AbstractDevice;
|
||||||
|
using cmvr::device::DeviceHealthSnapshot;
|
||||||
|
using cmvr::device::DeviceHealthState;
|
||||||
|
using cmvr::device::DeviceKind;
|
||||||
|
using cmvr::device::DeviceManager;
|
||||||
|
using cmvr::device::DeviceManagerSnapshot;
|
||||||
|
using cmvr::device::ManagedDeviceSnapshot;
|
||||||
|
using cmvr::device::ManagedDeviceState;
|
||||||
|
|
||||||
|
class MemoryCamera final : public cmvr::device::AbstractCamera {
|
||||||
|
public:
|
||||||
|
std::string typeName() const override { return "MemoryCamera"; }
|
||||||
|
void getState(cmvr::device::CameraState& output) override
|
||||||
|
{
|
||||||
|
output = state;
|
||||||
|
}
|
||||||
|
|
||||||
|
cmvr::device::CameraState state{};
|
||||||
|
};
|
||||||
|
|
||||||
|
class MemoryMicrophone final : public cmvr::device::AbstractMicrophone {
|
||||||
|
public:
|
||||||
|
std::string typeName() const override { return "MemoryMicrophone"; }
|
||||||
|
void getState(cmvr::device::MicrophoneState& output) override
|
||||||
|
{
|
||||||
|
output = state;
|
||||||
|
}
|
||||||
|
|
||||||
|
cmvr::device::MicrophoneState state{};
|
||||||
|
};
|
||||||
|
|
||||||
|
class MemoryDexHand final : public cmvr::device::AbstractDexHand {
|
||||||
|
public:
|
||||||
|
std::string typeName() const override { return "MemoryDexHand"; }
|
||||||
|
Status state() const override { return lifecycle; }
|
||||||
|
std::string lastError() const override { return error; }
|
||||||
|
void setAngles(const std::vector<int>&) override {}
|
||||||
|
void setTactilePollingRegions(
|
||||||
|
const std::vector<TactileRegionKey>&) override {}
|
||||||
|
std::vector<TactileRegionData> getSensorData() override { return {}; }
|
||||||
|
TactileRegionData getSensorData(FingerType, TactileRegion) override
|
||||||
|
{
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
ResultantForce getResultantForce(FingerType, TactileRegion) override
|
||||||
|
{
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
|
||||||
|
Status lifecycle{Status::CREATED};
|
||||||
|
std::string error;
|
||||||
|
};
|
||||||
|
|
||||||
|
class FakeDevice final : public AbstractDevice {
|
||||||
|
public:
|
||||||
|
explicit FakeDevice(std::string id,
|
||||||
|
DeviceKind kind = DeviceKind::Camera)
|
||||||
|
: AbstractDevice(std::move(id)), kind_(kind)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
DeviceKind kind() const noexcept override { return kind_; }
|
||||||
|
std::string typeName() const override { return "FakeDevice"; }
|
||||||
|
|
||||||
|
bool start() override
|
||||||
|
{
|
||||||
|
++start_calls;
|
||||||
|
if (throw_on_start) {
|
||||||
|
throw std::runtime_error(std::string(700, 's'));
|
||||||
|
}
|
||||||
|
return start_result;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool stop() override
|
||||||
|
{
|
||||||
|
++stop_calls;
|
||||||
|
if (throw_on_stop) {
|
||||||
|
throw std::runtime_error(std::string(700, 't'));
|
||||||
|
}
|
||||||
|
return stop_result;
|
||||||
|
}
|
||||||
|
|
||||||
|
DeviceHealthSnapshot healthSnapshot() override
|
||||||
|
{
|
||||||
|
++health_calls;
|
||||||
|
if (throw_on_health) {
|
||||||
|
throw std::runtime_error(std::string(700, 'h'));
|
||||||
|
}
|
||||||
|
return health;
|
||||||
|
}
|
||||||
|
|
||||||
|
DeviceKind kind_;
|
||||||
|
bool start_result{true};
|
||||||
|
bool stop_result{true};
|
||||||
|
bool throw_on_start{false};
|
||||||
|
bool throw_on_stop{false};
|
||||||
|
bool throw_on_health{false};
|
||||||
|
DeviceHealthSnapshot health{DeviceHealthState::Healthy, {}};
|
||||||
|
std::atomic<int> start_calls{0};
|
||||||
|
std::atomic<int> stop_calls{0};
|
||||||
|
std::atomic<int> health_calls{0};
|
||||||
|
};
|
||||||
|
|
||||||
|
const ManagedDeviceSnapshot* findDevice(const DeviceManagerSnapshot& snapshot,
|
||||||
|
const std::string& id)
|
||||||
|
{
|
||||||
|
for (const auto& device : snapshot.devices) {
|
||||||
|
if (device.id == id) {
|
||||||
|
return &device;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isSorted(const DeviceManagerSnapshot& snapshot)
|
||||||
|
{
|
||||||
|
for (std::size_t i = 1; i < snapshot.devices.size(); ++i) {
|
||||||
|
if (snapshot.devices[i].id < snapshot.devices[i - 1].id) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool testCategoryHealthAdapters()
|
||||||
|
{
|
||||||
|
MemoryCamera camera;
|
||||||
|
CHECK_TRUE(camera.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Unknown);
|
||||||
|
camera.state.is_initialized = true;
|
||||||
|
CHECK_TRUE(camera.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Healthy);
|
||||||
|
camera.state.error_message = "camera warning";
|
||||||
|
CHECK_TRUE(camera.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Degraded);
|
||||||
|
camera.state.is_error = true;
|
||||||
|
CHECK_TRUE(camera.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Fault);
|
||||||
|
|
||||||
|
MemoryMicrophone microphone;
|
||||||
|
microphone.state.is_initialized = true;
|
||||||
|
CHECK_TRUE(microphone.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Healthy);
|
||||||
|
microphone.state.is_error = true;
|
||||||
|
microphone.state.error_message = "microphone fault";
|
||||||
|
const auto microphone_health = microphone.healthSnapshot();
|
||||||
|
CHECK_TRUE(microphone_health.state == DeviceHealthState::Fault);
|
||||||
|
CHECK_TRUE(microphone_health.error_message == "microphone fault");
|
||||||
|
|
||||||
|
MemoryDexHand dexhand;
|
||||||
|
CHECK_TRUE(dexhand.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Unknown);
|
||||||
|
dexhand.lifecycle = MemoryDexHand::Status::INITIALIZED;
|
||||||
|
CHECK_TRUE(dexhand.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Healthy);
|
||||||
|
dexhand.error = "temporary warning";
|
||||||
|
CHECK_TRUE(dexhand.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Degraded);
|
||||||
|
dexhand.lifecycle = MemoryDexHand::Status::FAULT;
|
||||||
|
CHECK_TRUE(dexhand.healthSnapshot().state ==
|
||||||
|
DeviceHealthState::Fault);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool testConfiguredAndDynamicSnapshots()
|
||||||
|
{
|
||||||
|
cmvr::config::DeviceManagerConfig config;
|
||||||
|
config.set_name("snapshot-test");
|
||||||
|
config.set_version("9.1");
|
||||||
|
config.set_description("device manager snapshot test");
|
||||||
|
|
||||||
|
auto* disabled = config.add_devices();
|
||||||
|
disabled->set_id("disabled_camera");
|
||||||
|
disabled->set_type(
|
||||||
|
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
|
||||||
|
disabled->set_enable(false);
|
||||||
|
|
||||||
|
auto* broken = config.add_devices();
|
||||||
|
broken->set_id("broken_device");
|
||||||
|
broken->set_type(
|
||||||
|
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN);
|
||||||
|
broken->set_enable(true);
|
||||||
|
|
||||||
|
auto* duplicate_disabled = config.add_devices();
|
||||||
|
duplicate_disabled->set_id("duplicate_device");
|
||||||
|
duplicate_disabled->set_type(
|
||||||
|
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
|
||||||
|
duplicate_disabled->set_enable(false);
|
||||||
|
|
||||||
|
auto* duplicate_enabled = config.add_devices();
|
||||||
|
duplicate_enabled->set_id("duplicate_device");
|
||||||
|
duplicate_enabled->set_type(
|
||||||
|
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
|
||||||
|
duplicate_enabled->set_enable(true);
|
||||||
|
|
||||||
|
auto& manager = DeviceManager::getInstance(config);
|
||||||
|
auto configured = manager.snapshot();
|
||||||
|
CHECK_TRUE(configured.name == "snapshot-test");
|
||||||
|
CHECK_TRUE(configured.version == "9.1");
|
||||||
|
CHECK_TRUE(configured.description == "device manager snapshot test");
|
||||||
|
CHECK_TRUE(configured.devices.size() == 3);
|
||||||
|
CHECK_TRUE(isSorted(configured));
|
||||||
|
|
||||||
|
const auto* disabled_status =
|
||||||
|
findDevice(configured, "disabled_camera");
|
||||||
|
CHECK_TRUE(disabled_status != nullptr);
|
||||||
|
CHECK_TRUE(!disabled_status->enabled);
|
||||||
|
CHECK_TRUE(disabled_status->kind == DeviceKind::Camera);
|
||||||
|
CHECK_TRUE(disabled_status->type_name == "Camera");
|
||||||
|
CHECK_TRUE(disabled_status->state == ManagedDeviceState::Disabled);
|
||||||
|
CHECK_TRUE(disabled_status->health.state ==
|
||||||
|
DeviceHealthState::Unknown);
|
||||||
|
CHECK_TRUE(!disabled_status->abnormal);
|
||||||
|
CHECK_TRUE(disabled_status->status_updated_at_unix_ms != 0);
|
||||||
|
|
||||||
|
const auto* broken_status = findDevice(configured, "broken_device");
|
||||||
|
CHECK_TRUE(broken_status != nullptr);
|
||||||
|
CHECK_TRUE(broken_status->enabled);
|
||||||
|
CHECK_TRUE(broken_status->state == ManagedDeviceState::Error);
|
||||||
|
CHECK_TRUE(broken_status->health.state == DeviceHealthState::Unknown);
|
||||||
|
CHECK_TRUE(broken_status->abnormal);
|
||||||
|
CHECK_TRUE(!broken_status->error_message.empty());
|
||||||
|
CHECK_TRUE(broken_status->error_message.size() <= 512);
|
||||||
|
|
||||||
|
const auto* duplicate_status =
|
||||||
|
findDevice(configured, "duplicate_device");
|
||||||
|
CHECK_TRUE(duplicate_status != nullptr);
|
||||||
|
CHECK_TRUE(duplicate_status->enabled);
|
||||||
|
CHECK_TRUE(duplicate_status->state == ManagedDeviceState::Error);
|
||||||
|
CHECK_TRUE(duplicate_status->abnormal);
|
||||||
|
CHECK_TRUE(duplicate_status->error_message ==
|
||||||
|
"duplicate configured device id: duplicate_device");
|
||||||
|
|
||||||
|
auto healthy = std::make_shared<FakeDevice>("z_healthy");
|
||||||
|
auto degraded = std::make_shared<FakeDevice>("a_degraded");
|
||||||
|
degraded->health = {
|
||||||
|
DeviceHealthState::Degraded, std::string(700, 'd')};
|
||||||
|
auto start_fail = std::make_shared<FakeDevice>("m_start_fail");
|
||||||
|
start_fail->start_result = false;
|
||||||
|
auto stop_fail = std::make_shared<FakeDevice>("n_stop_fail");
|
||||||
|
stop_fail->stop_result = false;
|
||||||
|
auto health_throw = std::make_shared<FakeDevice>("b_health_throw");
|
||||||
|
health_throw->throw_on_health = true;
|
||||||
|
|
||||||
|
manager.registerDevice(healthy);
|
||||||
|
manager.registerDevice(degraded);
|
||||||
|
manager.registerDevice(start_fail);
|
||||||
|
manager.registerDevice(stop_fail);
|
||||||
|
manager.registerDevice(health_throw);
|
||||||
|
|
||||||
|
// Duplicate registration must retain the original object and status.
|
||||||
|
manager.registerDevice(
|
||||||
|
std::make_shared<FakeDevice>("z_healthy", DeviceKind::Speaker));
|
||||||
|
CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy);
|
||||||
|
|
||||||
|
const auto registered = manager.snapshot();
|
||||||
|
CHECK_TRUE(isSorted(registered));
|
||||||
|
const auto* healthy_registered =
|
||||||
|
findDevice(registered, "z_healthy");
|
||||||
|
CHECK_TRUE(healthy_registered != nullptr);
|
||||||
|
CHECK_TRUE(healthy_registered->state ==
|
||||||
|
ManagedDeviceState::Registered);
|
||||||
|
CHECK_TRUE(healthy_registered->health.state ==
|
||||||
|
DeviceHealthState::Healthy);
|
||||||
|
CHECK_TRUE(!healthy_registered->abnormal);
|
||||||
|
|
||||||
|
const auto* degraded_registered =
|
||||||
|
findDevice(registered, "a_degraded");
|
||||||
|
CHECK_TRUE(degraded_registered != nullptr);
|
||||||
|
CHECK_TRUE(degraded_registered->abnormal);
|
||||||
|
CHECK_TRUE(degraded_registered->health.state ==
|
||||||
|
DeviceHealthState::Degraded);
|
||||||
|
CHECK_TRUE(degraded_registered->health.error_message.size() == 512);
|
||||||
|
CHECK_TRUE(degraded_registered->error_message.size() == 512);
|
||||||
|
|
||||||
|
const auto* thrown_health =
|
||||||
|
findDevice(registered, "b_health_throw");
|
||||||
|
CHECK_TRUE(thrown_health != nullptr);
|
||||||
|
CHECK_TRUE(thrown_health->abnormal);
|
||||||
|
CHECK_TRUE(thrown_health->health.state ==
|
||||||
|
DeviceHealthState::Fault);
|
||||||
|
CHECK_TRUE(thrown_health->health.error_message.size() <= 512);
|
||||||
|
CHECK_TRUE(thrown_health->error_message.size() <= 512);
|
||||||
|
|
||||||
|
manager.start();
|
||||||
|
const auto running = manager.snapshot();
|
||||||
|
CHECK_TRUE(findDevice(running, "z_healthy")->state ==
|
||||||
|
ManagedDeviceState::Running);
|
||||||
|
CHECK_TRUE(findDevice(running, "m_start_fail")->state ==
|
||||||
|
ManagedDeviceState::Error);
|
||||||
|
CHECK_TRUE(findDevice(running, "m_start_fail")->abnormal);
|
||||||
|
CHECK_TRUE(findDevice(running, "m_start_fail")->health.state ==
|
||||||
|
DeviceHealthState::Healthy);
|
||||||
|
CHECK_TRUE(healthy->start_calls.load() == 1);
|
||||||
|
|
||||||
|
// The earlier value snapshot remains independent from manager mutations.
|
||||||
|
CHECK_TRUE(healthy_registered->state ==
|
||||||
|
ManagedDeviceState::Registered);
|
||||||
|
|
||||||
|
manager.stop();
|
||||||
|
const auto stopped = manager.snapshot();
|
||||||
|
CHECK_TRUE(findDevice(stopped, "z_healthy")->state ==
|
||||||
|
ManagedDeviceState::Stopped);
|
||||||
|
CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state ==
|
||||||
|
ManagedDeviceState::Error);
|
||||||
|
CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal);
|
||||||
|
CHECK_TRUE(healthy->stop_calls.load() == 1);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool testConcurrentSnapshotAndRegistration()
|
||||||
|
{
|
||||||
|
auto& manager = DeviceManager::getInstance();
|
||||||
|
std::atomic<bool> done{false};
|
||||||
|
std::atomic<bool> reader_ok{true};
|
||||||
|
|
||||||
|
std::thread reader([&] {
|
||||||
|
while (!done.load(std::memory_order_acquire)) {
|
||||||
|
const auto current = manager.snapshot();
|
||||||
|
if (!isSorted(current)) {
|
||||||
|
reader_ok.store(false, std::memory_order_release);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
for (int i = 0; i < 32; ++i) {
|
||||||
|
manager.registerDevice(
|
||||||
|
std::make_shared<FakeDevice>(
|
||||||
|
"concurrent_" + std::to_string(i)));
|
||||||
|
}
|
||||||
|
done.store(true, std::memory_order_release);
|
||||||
|
reader.join();
|
||||||
|
|
||||||
|
CHECK_TRUE(reader_ok.load(std::memory_order_acquire));
|
||||||
|
const auto final_snapshot = manager.snapshot();
|
||||||
|
CHECK_TRUE(isSorted(final_snapshot));
|
||||||
|
for (int i = 0; i < 32; ++i) {
|
||||||
|
CHECK_TRUE(
|
||||||
|
findDevice(final_snapshot,
|
||||||
|
"concurrent_" + std::to_string(i)) != nullptr);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
DeviceManager::destroyInstance();
|
||||||
|
const bool success =
|
||||||
|
testCategoryHealthAdapters() &&
|
||||||
|
testConfiguredAndDynamicSnapshots() &&
|
||||||
|
testConcurrentSnapshotAndRegistration();
|
||||||
|
DeviceManager::destroyInstance();
|
||||||
|
return success ? 0 : 1;
|
||||||
|
}
|
||||||
61
cmvr-es/manager/media_source_hub/CMakeLists.txt
Normal file
61
cmvr-es/manager/media_source_hub/CMakeLists.txt
Normal file
@ -0,0 +1,61 @@
|
|||||||
|
if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
|
||||||
|
cmake_minimum_required(VERSION 3.22)
|
||||||
|
project(cmvr_media_source_hub LANGUAGES CXX)
|
||||||
|
enable_testing()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
add_library(media_source_hub STATIC
|
||||||
|
src/media_source_hub.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_compile_features(media_source_hub PUBLIC cxx_std_17)
|
||||||
|
target_include_directories(media_source_hub
|
||||||
|
PUBLIC
|
||||||
|
${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(cmvr_es::media_source_hub ALIAS media_source_hub)
|
||||||
|
|
||||||
|
if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
|
||||||
|
add_library(device_media_source_adapter STATIC
|
||||||
|
src/device_media_source_adapter.cpp
|
||||||
|
)
|
||||||
|
target_compile_features(device_media_source_adapter PUBLIC cxx_std_17)
|
||||||
|
target_include_directories(device_media_source_adapter
|
||||||
|
PUBLIC
|
||||||
|
${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||||
|
)
|
||||||
|
target_link_libraries(device_media_source_adapter
|
||||||
|
PUBLIC
|
||||||
|
cmvr_es::media_source_hub
|
||||||
|
cmvr_es::common
|
||||||
|
cmvr_es::proto
|
||||||
|
cmvr_es::logging
|
||||||
|
)
|
||||||
|
add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
option(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS
|
||||||
|
"Build the standalone MediaSourceHub self-test"
|
||||||
|
${PROJECT_IS_TOP_LEVEL})
|
||||||
|
|
||||||
|
if(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS)
|
||||||
|
find_package(Threads REQUIRED)
|
||||||
|
add_executable(media_source_hub_test
|
||||||
|
tests/media_source_hub_test.cpp
|
||||||
|
)
|
||||||
|
target_compile_features(media_source_hub_test PRIVATE cxx_std_17)
|
||||||
|
target_link_libraries(media_source_hub_test
|
||||||
|
PRIVATE
|
||||||
|
cmvr_es::media_source_hub
|
||||||
|
Threads::Threads
|
||||||
|
)
|
||||||
|
# This self-test only links the static Hub and pthreads. In the root build,
|
||||||
|
# the project-wide third-party RUNPATH can otherwise make the loader pick up
|
||||||
|
# a vendor libstdc++.so (for example from the AUBO SDK), even though the test
|
||||||
|
# has no dependency on that SDK.
|
||||||
|
set_target_properties(media_source_hub_test PROPERTIES
|
||||||
|
SKIP_BUILD_RPATH TRUE
|
||||||
|
)
|
||||||
|
add_test(NAME media_source_hub_test COMMAND media_source_hub_test)
|
||||||
|
endif()
|
||||||
@ -0,0 +1,38 @@
|
|||||||
|
#ifndef CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||||
|
#define CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstddef>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "devices/camera/abstract_camera.h"
|
||||||
|
#include "devices/microphone/abstract_microphone.h"
|
||||||
|
#include "manager/media_source_hub/include/media_source_hub.h"
|
||||||
|
|
||||||
|
namespace cmvr::media {
|
||||||
|
|
||||||
|
// Process-wide protocol-neutral media hub shared by gRPC and QUIC services.
|
||||||
|
MediaSourceHub& globalMediaSourceHub();
|
||||||
|
|
||||||
|
std::string cameraColorTrackId(const std::string& device_id);
|
||||||
|
std::string microphoneTrackId(const std::string& device_id);
|
||||||
|
|
||||||
|
// Registration is idempotent for an already registered track. The adapter owns a
|
||||||
|
// short-lived pump thread and one startStreaming()/stopStreaming() lease only while
|
||||||
|
// at least one Hub subscription is active. It ensures start() succeeds but deliberately
|
||||||
|
// does not call stop(), because the base device lifecycle can also be owned by control RPCs.
|
||||||
|
bool ensureCameraMediaSource(
|
||||||
|
MediaSourceHub& hub,
|
||||||
|
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||||
|
size_t ring_capacity = 64);
|
||||||
|
|
||||||
|
bool ensureMicrophoneMediaSource(
|
||||||
|
MediaSourceHub& hub,
|
||||||
|
const std::shared_ptr<device::AbstractMicrophone>& microphone,
|
||||||
|
size_t ring_capacity = 256);
|
||||||
|
|
||||||
|
} // namespace cmvr::media
|
||||||
|
|
||||||
|
#endif // CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H
|
||||||
128
cmvr-es/manager/media_source_hub/include/media_source_hub.h
Normal file
128
cmvr-es/manager/media_source_hub/include/media_source_hub.h
Normal file
@ -0,0 +1,128 @@
|
|||||||
|
#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
|
||||||
|
#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <functional>
|
||||||
|
#include <memory>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/base/ring_buffer.h"
|
||||||
|
#include "common/media/media_frame.h"
|
||||||
|
|
||||||
|
namespace cmvr::media {
|
||||||
|
|
||||||
|
// MediaSourceHub owns no protocol-specific state. A device or capture adapter registers
|
||||||
|
// start/stop callbacks and receives a sink callback when the first consumer subscribes.
|
||||||
|
class MediaSourceHub final {
|
||||||
|
public:
|
||||||
|
using FrameRing = BroadcastFrameRing<MediaFrame>;
|
||||||
|
using FrameReadResult = FrameRing::ReadResult;
|
||||||
|
using StartPosition = FrameRing::StartPosition;
|
||||||
|
using FrameSink = std::function<void(MediaFramePtr)>;
|
||||||
|
// Cancellation checks run while MediaSourceHub protects source lifecycle
|
||||||
|
// state. Predicates must therefore be fast, non-blocking and must not call
|
||||||
|
// back into the same hub.
|
||||||
|
using CancelPredicate = std::function<bool()>;
|
||||||
|
|
||||||
|
struct SourceCallbacks {
|
||||||
|
// start() may run asynchronously. It must observe cancelled during any
|
||||||
|
// potentially blocking startup work and return false promptly once set.
|
||||||
|
// MediaSourceHub retains the callback state until a non-cooperative start
|
||||||
|
// eventually returns, so late completion cannot access destroyed state.
|
||||||
|
std::function<bool(
|
||||||
|
const FrameSink& sink,
|
||||||
|
const CancelPredicate& cancelled)> start;
|
||||||
|
// stop() is the synchronous publication barrier for the last lease and
|
||||||
|
// must unblock and join the source producer before returning.
|
||||||
|
std::function<void()> stop;
|
||||||
|
std::function<bool()> request_key_frame;
|
||||||
|
};
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct SourceState;
|
||||||
|
|
||||||
|
public:
|
||||||
|
class Subscription final {
|
||||||
|
public:
|
||||||
|
Subscription() = default;
|
||||||
|
~Subscription();
|
||||||
|
|
||||||
|
Subscription(const Subscription&) = delete;
|
||||||
|
Subscription& operator=(const Subscription&) = delete;
|
||||||
|
Subscription(Subscription&& other) noexcept;
|
||||||
|
Subscription& operator=(Subscription&& other) noexcept;
|
||||||
|
|
||||||
|
// A Subscription owns one reader cursor and is single-consumer. Moving,
|
||||||
|
// resetting, or reading the same object concurrently is unsupported; use
|
||||||
|
// one independent subscription per consumer thread.
|
||||||
|
bool valid() const;
|
||||||
|
explicit operator bool() const { return valid(); }
|
||||||
|
|
||||||
|
// Returns the most recently observed immutable descriptor. A callback source may
|
||||||
|
// replace the initially registered UNKNOWN codec/config descriptor with the first
|
||||||
|
// real frame descriptor without invalidating existing subscriptions.
|
||||||
|
TrackDescriptorPtr descriptor() const;
|
||||||
|
|
||||||
|
std::optional<FrameReadResult> tryRead();
|
||||||
|
std::optional<FrameReadResult> waitRead(std::chrono::milliseconds timeout);
|
||||||
|
uint64_t discardPendingIfExceeds(size_t maximum_pending_frames);
|
||||||
|
uint64_t droppedCount() const noexcept;
|
||||||
|
void reset();
|
||||||
|
|
||||||
|
private:
|
||||||
|
friend class MediaSourceHub;
|
||||||
|
Subscription(std::shared_ptr<SourceState> source, FrameRing::Cursor cursor);
|
||||||
|
|
||||||
|
std::shared_ptr<SourceState> source_;
|
||||||
|
FrameRing::Cursor cursor_;
|
||||||
|
bool active_{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
MediaSourceHub();
|
||||||
|
~MediaSourceHub();
|
||||||
|
|
||||||
|
MediaSourceHub(const MediaSourceHub&) = delete;
|
||||||
|
MediaSourceHub& operator=(const MediaSourceHub&) = delete;
|
||||||
|
|
||||||
|
bool registerSource(
|
||||||
|
TrackDescriptorPtr initial_descriptor,
|
||||||
|
SourceCallbacks callbacks,
|
||||||
|
size_t ring_capacity = 64);
|
||||||
|
|
||||||
|
// Active sources cannot be unregistered. Destroy/reset their subscriptions first.
|
||||||
|
bool unregisterSource(const std::string& track_id);
|
||||||
|
|
||||||
|
bool hasSource(const std::string& track_id) const;
|
||||||
|
std::vector<TrackDescriptorPtr> listTracks() const;
|
||||||
|
size_t subscriberCount(const std::string& track_id) const;
|
||||||
|
|
||||||
|
// Protocol adapters can request an IDR after a discontinuity without knowing the
|
||||||
|
// concrete camera implementation. Returns false when unsupported or not running.
|
||||||
|
bool requestKeyFrame(const std::string& track_id) const;
|
||||||
|
|
||||||
|
Subscription subscribe(
|
||||||
|
const std::string& track_id,
|
||||||
|
StartPosition start_position = StartPosition::NEXT_PUBLISHED,
|
||||||
|
CancelPredicate cancelled = {});
|
||||||
|
|
||||||
|
// Stops all registered sources and invalidates outstanding subscriptions. The
|
||||||
|
// subscriptions remain destructible and their waitRead calls are awakened.
|
||||||
|
// A cooperative in-progress start is cancelled; a callback that violates the
|
||||||
|
// cancellation contract is quarantined with retained state rather than blocking
|
||||||
|
// shutdown or risking a use-after-free.
|
||||||
|
void shutdown();
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Impl;
|
||||||
|
std::shared_ptr<Impl> impl_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::media
|
||||||
|
|
||||||
|
#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H
|
||||||
@ -0,0 +1,685 @@
|
|||||||
|
#include "manager/media_source_hub/include/device_media_source_adapter.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cctype>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <exception>
|
||||||
|
#include <iterator>
|
||||||
|
#include <limits>
|
||||||
|
#include <mutex>
|
||||||
|
#include <thread>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "common/base/logging/logger.h"
|
||||||
|
|
||||||
|
namespace cmvr::media {
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
std::string normalizedCodec(std::string codec) {
|
||||||
|
codec.erase(
|
||||||
|
std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) {
|
||||||
|
return !std::isalnum(c);
|
||||||
|
}),
|
||||||
|
codec.end());
|
||||||
|
std::transform(codec.begin(), codec.end(), codec.begin(), [](const unsigned char c) {
|
||||||
|
return static_cast<char>(std::tolower(c));
|
||||||
|
});
|
||||||
|
return codec;
|
||||||
|
}
|
||||||
|
|
||||||
|
Codec videoCodec(const std::string& value) {
|
||||||
|
const std::string codec = normalizedCodec(value);
|
||||||
|
if (codec == "h264" || codec == "avc" || codec == "avc1" || codec == "libx264") {
|
||||||
|
return Codec::H264;
|
||||||
|
}
|
||||||
|
if (codec == "h265" || codec == "hevc" || codec == "hvc1" || codec == "libx265") {
|
||||||
|
return Codec::H265;
|
||||||
|
}
|
||||||
|
return Codec::UNKNOWN;
|
||||||
|
}
|
||||||
|
|
||||||
|
PayloadFormat videoPayloadFormat(
|
||||||
|
const Codec codec,
|
||||||
|
const std::vector<uint8_t>& payload) noexcept {
|
||||||
|
if (codec != Codec::H264 && codec != Codec::H265) {
|
||||||
|
return PayloadFormat::UNKNOWN;
|
||||||
|
}
|
||||||
|
const bool three_byte_start_code = payload.size() >= 3 &&
|
||||||
|
payload[0] == 0U && payload[1] == 0U && payload[2] == 1U;
|
||||||
|
const bool four_byte_start_code = payload.size() >= 4 &&
|
||||||
|
payload[0] == 0U && payload[1] == 0U && payload[2] == 0U && payload[3] == 1U;
|
||||||
|
return three_byte_start_code || four_byte_start_code
|
||||||
|
? PayloadFormat::ANNEX_B
|
||||||
|
: PayloadFormat::UNKNOWN;
|
||||||
|
}
|
||||||
|
|
||||||
|
Codec audioCodec(const std::string& value) {
|
||||||
|
const std::string codec = normalizedCodec(value);
|
||||||
|
if (codec == "opus") return Codec::OPUS;
|
||||||
|
if (codec == "aac") return Codec::AAC;
|
||||||
|
if (codec == "pcms16le") return Codec::PCM_S16LE;
|
||||||
|
return Codec::UNKNOWN;
|
||||||
|
}
|
||||||
|
|
||||||
|
Codec audioCodec(const device::AudioStreamFrameData& source) {
|
||||||
|
const Codec codec = audioCodec(source.codec);
|
||||||
|
if (codec != Codec::UNKNOWN || !normalizedCodec(source.codec).empty()) {
|
||||||
|
return codec;
|
||||||
|
}
|
||||||
|
switch (source.format) {
|
||||||
|
case device::AudioStreamFormat::PCM:
|
||||||
|
return Codec::PCM_S16LE;
|
||||||
|
case device::AudioStreamFormat::AAC:
|
||||||
|
return Codec::AAC;
|
||||||
|
case device::AudioStreamFormat::OPUS:
|
||||||
|
return Codec::OPUS;
|
||||||
|
default:
|
||||||
|
return Codec::UNKNOWN;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
PayloadFormat audioPayloadFormat(
|
||||||
|
const Codec codec,
|
||||||
|
const std::vector<uint8_t>& payload) noexcept {
|
||||||
|
switch (codec) {
|
||||||
|
case Codec::OPUS:
|
||||||
|
return PayloadFormat::OPUS_PACKET;
|
||||||
|
case Codec::PCM_S16LE:
|
||||||
|
return PayloadFormat::RAW;
|
||||||
|
case Codec::AAC: {
|
||||||
|
// FFmpeg encoders commonly expose raw AAC access units plus AudioSpecificConfig;
|
||||||
|
// only advertise ADTS when the sync word and layer bits are actually present.
|
||||||
|
const bool has_adts_header = payload.size() >= 2 && payload[0] == 0xFFU &&
|
||||||
|
(payload[1] & 0xF6U) == 0xF0U;
|
||||||
|
return has_adts_header ? PayloadFormat::AAC_ADTS : PayloadFormat::RAW;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
return PayloadFormat::UNKNOWN;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Rational sanitizedTimeBase(
|
||||||
|
const int32_t numerator,
|
||||||
|
const int32_t denominator,
|
||||||
|
const int32_t fallback_denominator) noexcept {
|
||||||
|
return Rational{
|
||||||
|
numerator > 0 ? numerator : 1,
|
||||||
|
denominator > 0 ? denominator : std::max(1, fallback_denominator)};
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t descriptorGeneration(
|
||||||
|
const uint64_t stream_epoch,
|
||||||
|
const uint32_t codec_generation) {
|
||||||
|
const uint64_t generation = (stream_epoch << 32U) | codec_generation;
|
||||||
|
return generation == 0 ? 1 : generation;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<typename DeviceT>
|
||||||
|
struct PumpState : public std::enable_shared_from_this<PumpState<DeviceT>> {
|
||||||
|
explicit PumpState(std::shared_ptr<DeviceT> device_ptr)
|
||||||
|
: device(std::move(device_ptr)) {}
|
||||||
|
|
||||||
|
virtual ~PumpState() {
|
||||||
|
stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool begin(
|
||||||
|
const MediaSourceHub::FrameSink& frame_sink,
|
||||||
|
const MediaSourceHub::CancelPredicate& cancelled) {
|
||||||
|
if (!frame_sink || !device) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const auto cancellation_requested = [&cancelled] {
|
||||||
|
if (!cancelled) return false;
|
||||||
|
try {
|
||||||
|
return cancelled();
|
||||||
|
} catch (...) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
if (cancellation_requested()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::unique_lock<std::mutex> lock(mutex);
|
||||||
|
if (running.load(std::memory_order_acquire)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (worker.joinable()) {
|
||||||
|
// A previous worker must always be collected before a new capture lease starts.
|
||||||
|
std::thread stale_worker = std::move(worker);
|
||||||
|
lock.unlock();
|
||||||
|
collectThread(std::move(stale_worker));
|
||||||
|
lock.lock();
|
||||||
|
}
|
||||||
|
|
||||||
|
sink = frame_sink;
|
||||||
|
bool streaming_attempted = false;
|
||||||
|
try {
|
||||||
|
if (!device->start()) {
|
||||||
|
sink = {};
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (cancellation_requested()) {
|
||||||
|
sink = {};
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
streaming_attempted = true;
|
||||||
|
if (!device->startStreaming()) {
|
||||||
|
sink = {};
|
||||||
|
lock.unlock();
|
||||||
|
stopDeviceStreaming();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
streaming_started = true;
|
||||||
|
if (cancellation_requested()) {
|
||||||
|
streaming_started = false;
|
||||||
|
sink = {};
|
||||||
|
lock.unlock();
|
||||||
|
stopDeviceStreaming();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
running.store(true, std::memory_order_release);
|
||||||
|
try {
|
||||||
|
// The worker owns the pump while run() is active. This also
|
||||||
|
// makes the defensive self-stop/detach path lifetime-safe.
|
||||||
|
const auto self = this->shared_from_this();
|
||||||
|
worker = std::thread([self] { self->run(); });
|
||||||
|
} catch (...) {
|
||||||
|
running.store(false, std::memory_order_release);
|
||||||
|
streaming_started = false;
|
||||||
|
sink = {};
|
||||||
|
lock.unlock();
|
||||||
|
stopDeviceStreaming();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source: "
|
||||||
|
<< error.what();
|
||||||
|
sink = {};
|
||||||
|
lock.unlock();
|
||||||
|
if (streaming_attempted) stopDeviceStreaming();
|
||||||
|
return false;
|
||||||
|
} catch (...) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source";
|
||||||
|
sink = {};
|
||||||
|
lock.unlock();
|
||||||
|
if (streaming_attempted) stopDeviceStreaming();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void stop() noexcept {
|
||||||
|
std::thread thread;
|
||||||
|
bool stop_streaming = false;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex);
|
||||||
|
running.store(false, std::memory_order_release);
|
||||||
|
stop_streaming = streaming_started;
|
||||||
|
streaming_started = false;
|
||||||
|
sink = {};
|
||||||
|
thread = std::move(worker);
|
||||||
|
}
|
||||||
|
if (stop_streaming) {
|
||||||
|
stopDeviceStreaming();
|
||||||
|
}
|
||||||
|
if (thread.joinable()) {
|
||||||
|
collectThread(std::move(thread));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void collectThread(std::thread thread) noexcept {
|
||||||
|
if (!thread.joinable()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
if (thread.get_id() == std::this_thread::get_id()) {
|
||||||
|
thread.detach();
|
||||||
|
} else {
|
||||||
|
thread.join();
|
||||||
|
}
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: "
|
||||||
|
<< error.what();
|
||||||
|
if (thread.joinable()) {
|
||||||
|
try {
|
||||||
|
thread.detach();
|
||||||
|
} catch (...) {
|
||||||
|
// std::thread's destructor would terminate if this extremely rare
|
||||||
|
// platform error occurred; there is no recoverable ownership path.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void run() = 0;
|
||||||
|
|
||||||
|
void stopDeviceStreaming() noexcept {
|
||||||
|
try {
|
||||||
|
if (device) {
|
||||||
|
device->stopStreaming();
|
||||||
|
}
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source: "
|
||||||
|
<< error.what();
|
||||||
|
} catch (...) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<DeviceT> device;
|
||||||
|
std::atomic<bool> running{false};
|
||||||
|
std::mutex mutex;
|
||||||
|
std::thread worker;
|
||||||
|
MediaSourceHub::FrameSink sink;
|
||||||
|
bool streaming_started{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct CameraPump final : PumpState<device::AbstractCamera> {
|
||||||
|
CameraPump(std::shared_ptr<device::AbstractCamera> camera, std::string id)
|
||||||
|
: PumpState(std::move(camera)), track_id(std::move(id)) {}
|
||||||
|
~CameraPump() override { stop(); }
|
||||||
|
|
||||||
|
void run() override {
|
||||||
|
size_t cursor = 0;
|
||||||
|
uint64_t last_epoch = 0;
|
||||||
|
uint64_t last_sequence = 0;
|
||||||
|
uint64_t cached_config_generation = 0;
|
||||||
|
std::vector<uint8_t> cached_codec_config;
|
||||||
|
TrackDescriptorPtr last_descriptor;
|
||||||
|
bool have_previous = false;
|
||||||
|
bool have_cached_config_generation = false;
|
||||||
|
bool waiting_for_key_frame = true;
|
||||||
|
bool pending_discontinuity = true;
|
||||||
|
bool requested_key_frame = false;
|
||||||
|
std::chrono::steady_clock::time_point last_key_frame_request;
|
||||||
|
|
||||||
|
const auto request_key_frame = [&] {
|
||||||
|
const auto now = std::chrono::steady_clock::now();
|
||||||
|
if (requested_key_frame &&
|
||||||
|
now - last_key_frame_request < std::chrono::milliseconds(250)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
requested_key_frame = true;
|
||||||
|
last_key_frame_request = now;
|
||||||
|
try {
|
||||||
|
device->requestKeyFrame();
|
||||||
|
} catch (...) {
|
||||||
|
// Unsupported/failed key-frame requests fall back to the encoder's GOP.
|
||||||
|
}
|
||||||
|
};
|
||||||
|
request_key_frame();
|
||||||
|
|
||||||
|
while (running.load(std::memory_order_acquire)) {
|
||||||
|
device::StreamFrameData source;
|
||||||
|
try {
|
||||||
|
if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||||
|
continue;
|
||||||
|
} catch (...) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for "
|
||||||
|
<< track_id;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (source.rgbFrame.empty()) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t descriptor_generation = descriptorGeneration(
|
||||||
|
source.stream_epoch, source.codec_config_generation);
|
||||||
|
if (!have_cached_config_generation ||
|
||||||
|
cached_config_generation != descriptor_generation) {
|
||||||
|
cached_codec_config.clear();
|
||||||
|
cached_config_generation = descriptor_generation;
|
||||||
|
have_cached_config_generation = true;
|
||||||
|
}
|
||||||
|
if (!source.codec_config.empty()) {
|
||||||
|
cached_codec_config = source.codec_config;
|
||||||
|
}
|
||||||
|
|
||||||
|
TrackDescriptor::Config track;
|
||||||
|
track.id = track_id;
|
||||||
|
track.source_id = device->id();
|
||||||
|
track.kind = MediaKind::VIDEO;
|
||||||
|
track.codec = videoCodec(source.codec);
|
||||||
|
track.payload_format = videoPayloadFormat(track.codec, source.rgbFrame);
|
||||||
|
track.time_base = sanitizedTimeBase(
|
||||||
|
source.time_base_num,
|
||||||
|
source.time_base_den,
|
||||||
|
source.fps);
|
||||||
|
track.width = static_cast<uint32_t>(std::max(0, source.width));
|
||||||
|
track.height = static_cast<uint32_t>(std::max(0, source.height));
|
||||||
|
track.nominal_rate = static_cast<uint32_t>(std::max(0, source.fps));
|
||||||
|
track.fx = source.intrinsics.fx;
|
||||||
|
track.fy = source.intrinsics.fy;
|
||||||
|
track.cx = source.intrinsics.cx;
|
||||||
|
track.cy = source.intrinsics.cy;
|
||||||
|
track.distortion.assign(
|
||||||
|
std::begin(source.intrinsics.coeffs),
|
||||||
|
std::end(source.intrinsics.coeffs));
|
||||||
|
track.generation = descriptor_generation;
|
||||||
|
track.codec_config = cached_codec_config;
|
||||||
|
|
||||||
|
const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
|
||||||
|
const bool sequence_wrapped = have_previous &&
|
||||||
|
last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
|
||||||
|
const bool sequence_gap = have_previous && !epoch_changed &&
|
||||||
|
(sequence_wrapped ||
|
||||||
|
(last_sequence != std::numeric_limits<uint64_t>::max() &&
|
||||||
|
source.sequence != last_sequence + 1));
|
||||||
|
|
||||||
|
TrackDescriptorPtr descriptor;
|
||||||
|
try {
|
||||||
|
descriptor = makeTrackDescriptor(std::move(track));
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera descriptor for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
pending_discontinuity = true;
|
||||||
|
last_epoch = source.stream_epoch;
|
||||||
|
last_sequence = source.sequence;
|
||||||
|
have_previous = true;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const bool descriptor_changed = last_descriptor &&
|
||||||
|
!equivalentTrackDescriptor(*last_descriptor, *descriptor);
|
||||||
|
const bool discontinuity = !have_previous || source.discontinuity || epoch_changed ||
|
||||||
|
sequence_gap || descriptor_changed;
|
||||||
|
const bool inter_frame_codec = descriptor->codec == Codec::H264 ||
|
||||||
|
descriptor->codec == Codec::H265;
|
||||||
|
|
||||||
|
if (discontinuity) {
|
||||||
|
pending_discontinuity = true;
|
||||||
|
if (inter_frame_codec) {
|
||||||
|
waiting_for_key_frame = true;
|
||||||
|
request_key_frame();
|
||||||
|
} else {
|
||||||
|
waiting_for_key_frame = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
last_epoch = source.stream_epoch;
|
||||||
|
last_sequence = source.sequence;
|
||||||
|
last_descriptor = descriptor;
|
||||||
|
have_previous = true;
|
||||||
|
|
||||||
|
if (waiting_for_key_frame && !source.bKey) {
|
||||||
|
request_key_frame();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
waiting_for_key_frame = false;
|
||||||
|
|
||||||
|
MediaFrame::Config frame;
|
||||||
|
frame.descriptor = std::move(descriptor);
|
||||||
|
frame.payload = std::move(source.rgbFrame);
|
||||||
|
frame.sequence = source.sequence;
|
||||||
|
frame.source_timestamp = source.source_timestamp;
|
||||||
|
frame.source_frame_number = source.source_frame_number;
|
||||||
|
frame.pts = source.pts;
|
||||||
|
frame.dts = source.dts;
|
||||||
|
frame.duration = source.duration;
|
||||||
|
frame.capture_time_ns = source.capture_monotonic_ns > 0
|
||||||
|
? static_cast<uint64_t>(source.capture_monotonic_ns)
|
||||||
|
: 0;
|
||||||
|
frame.capture_utc_ns = source.capture_utc_ns;
|
||||||
|
frame.key_frame = source.bKey;
|
||||||
|
frame.discontinuity = pending_discontinuity;
|
||||||
|
|
||||||
|
MediaSourceHub::FrameSink current_sink;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex);
|
||||||
|
current_sink = sink;
|
||||||
|
}
|
||||||
|
if (current_sink) {
|
||||||
|
try {
|
||||||
|
current_sink(makeMediaFrame(std::move(frame)));
|
||||||
|
pending_discontinuity = false;
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera frame for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
pending_discontinuity = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string track_id;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct MicrophonePump final : PumpState<device::AbstractMicrophone> {
|
||||||
|
MicrophonePump(std::shared_ptr<device::AbstractMicrophone> microphone, std::string id)
|
||||||
|
: PumpState(std::move(microphone)), track_id(std::move(id)) {}
|
||||||
|
~MicrophonePump() override { stop(); }
|
||||||
|
|
||||||
|
void run() override {
|
||||||
|
size_t cursor = 0;
|
||||||
|
uint64_t last_epoch = 0;
|
||||||
|
uint64_t last_sequence = 0;
|
||||||
|
uint64_t cached_config_generation = 0;
|
||||||
|
std::vector<uint8_t> cached_codec_config;
|
||||||
|
TrackDescriptorPtr last_descriptor;
|
||||||
|
bool have_previous = false;
|
||||||
|
bool have_cached_config_generation = false;
|
||||||
|
bool pending_discontinuity = true;
|
||||||
|
while (running.load(std::memory_order_acquire)) {
|
||||||
|
device::AudioStreamFrameData source;
|
||||||
|
try {
|
||||||
|
if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||||
|
continue;
|
||||||
|
} catch (...) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for "
|
||||||
|
<< track_id;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (source.data.empty()) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t descriptor_generation = descriptorGeneration(
|
||||||
|
source.stream_epoch, source.codec_config_generation);
|
||||||
|
if (!have_cached_config_generation ||
|
||||||
|
cached_config_generation != descriptor_generation) {
|
||||||
|
cached_codec_config.clear();
|
||||||
|
cached_config_generation = descriptor_generation;
|
||||||
|
have_cached_config_generation = true;
|
||||||
|
}
|
||||||
|
if (!source.codec_config.empty()) {
|
||||||
|
cached_codec_config = source.codec_config;
|
||||||
|
}
|
||||||
|
|
||||||
|
TrackDescriptor::Config track;
|
||||||
|
track.id = track_id;
|
||||||
|
track.source_id = device->id();
|
||||||
|
track.kind = MediaKind::AUDIO;
|
||||||
|
track.codec = audioCodec(source);
|
||||||
|
track.payload_format = audioPayloadFormat(track.codec, source.data);
|
||||||
|
track.time_base = sanitizedTimeBase(
|
||||||
|
source.time_base_num,
|
||||||
|
source.time_base_den,
|
||||||
|
source.sample_rate);
|
||||||
|
track.sample_rate = static_cast<uint32_t>(std::max(0, source.sample_rate));
|
||||||
|
track.channels = static_cast<uint32_t>(std::max(0, source.channels));
|
||||||
|
// Packet sample counts belong to MediaFrame::duration, not immutable track metadata.
|
||||||
|
track.nominal_rate = 0;
|
||||||
|
track.generation = descriptor_generation;
|
||||||
|
track.codec_config = cached_codec_config;
|
||||||
|
|
||||||
|
const bool epoch_changed = have_previous && source.stream_epoch != last_epoch;
|
||||||
|
const bool sequence_wrapped = have_previous &&
|
||||||
|
last_sequence == std::numeric_limits<uint64_t>::max() && source.sequence != 0;
|
||||||
|
const bool sequence_gap = have_previous && !epoch_changed &&
|
||||||
|
(sequence_wrapped ||
|
||||||
|
(last_sequence != std::numeric_limits<uint64_t>::max() &&
|
||||||
|
source.sequence != last_sequence + 1));
|
||||||
|
|
||||||
|
TrackDescriptorPtr descriptor;
|
||||||
|
try {
|
||||||
|
descriptor = makeTrackDescriptor(std::move(track));
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone descriptor for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
pending_discontinuity = true;
|
||||||
|
last_epoch = source.stream_epoch;
|
||||||
|
last_sequence = source.sequence;
|
||||||
|
have_previous = true;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const bool descriptor_changed = last_descriptor &&
|
||||||
|
!equivalentTrackDescriptor(*last_descriptor, *descriptor);
|
||||||
|
if (!have_previous || source.discontinuity || epoch_changed || sequence_gap ||
|
||||||
|
descriptor_changed) {
|
||||||
|
pending_discontinuity = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_epoch = source.stream_epoch;
|
||||||
|
last_sequence = source.sequence;
|
||||||
|
last_descriptor = descriptor;
|
||||||
|
have_previous = true;
|
||||||
|
|
||||||
|
MediaFrame::Config frame;
|
||||||
|
frame.descriptor = std::move(descriptor);
|
||||||
|
frame.payload = std::move(source.data);
|
||||||
|
frame.sequence = source.sequence;
|
||||||
|
frame.pts = source.pts;
|
||||||
|
frame.dts = source.dts;
|
||||||
|
frame.duration = source.duration > 0 ? source.duration : source.nb_samples;
|
||||||
|
frame.capture_time_ns = source.capture_monotonic_ns > 0
|
||||||
|
? static_cast<uint64_t>(source.capture_monotonic_ns)
|
||||||
|
: 0;
|
||||||
|
frame.capture_utc_ns = source.capture_utc_ns;
|
||||||
|
frame.key_frame = true;
|
||||||
|
frame.discontinuity = pending_discontinuity;
|
||||||
|
|
||||||
|
MediaSourceHub::FrameSink current_sink;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex);
|
||||||
|
current_sink = sink;
|
||||||
|
}
|
||||||
|
if (current_sink) {
|
||||||
|
try {
|
||||||
|
current_sink(makeMediaFrame(std::move(frame)));
|
||||||
|
pending_discontinuity = false;
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone frame for "
|
||||||
|
<< track_id << ": " << error.what();
|
||||||
|
pending_discontinuity = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string track_id;
|
||||||
|
};
|
||||||
|
|
||||||
|
TrackDescriptorPtr initialTrack(
|
||||||
|
std::string track_id,
|
||||||
|
std::string source_id,
|
||||||
|
const MediaKind kind) {
|
||||||
|
TrackDescriptor::Config config;
|
||||||
|
config.id = std::move(track_id);
|
||||||
|
config.source_id = std::move(source_id);
|
||||||
|
config.kind = kind;
|
||||||
|
// A placeholder descriptor is never emitted as a media sample, but it
|
||||||
|
// still carries a mathematically valid neutral time base.
|
||||||
|
config.time_base = Rational{1, 1};
|
||||||
|
config.generation = 1;
|
||||||
|
return makeTrackDescriptor(std::move(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
MediaSourceHub& globalMediaSourceHub() {
|
||||||
|
static MediaSourceHub hub;
|
||||||
|
return hub;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string cameraColorTrackId(const std::string& device_id) {
|
||||||
|
return device_id + "/video/color";
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string microphoneTrackId(const std::string& device_id) {
|
||||||
|
return device_id + "/audio/main";
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ensureCameraMediaSource(
|
||||||
|
MediaSourceHub& hub,
|
||||||
|
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||||
|
const size_t ring_capacity) {
|
||||||
|
if (!camera || camera->id().empty()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const std::string track_id = cameraColorTrackId(camera->id());
|
||||||
|
if (hub.hasSource(track_id)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto pump = std::make_shared<CameraPump>(camera, track_id);
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [pump](
|
||||||
|
const MediaSourceHub::FrameSink& sink,
|
||||||
|
const MediaSourceHub::CancelPredicate& cancelled) {
|
||||||
|
return pump->begin(sink, cancelled);
|
||||||
|
};
|
||||||
|
callbacks.stop = [pump] { pump->stop(); };
|
||||||
|
callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); };
|
||||||
|
const bool registered = hub.registerSource(
|
||||||
|
initialTrack(track_id, camera->id(), MediaKind::VIDEO),
|
||||||
|
std::move(callbacks),
|
||||||
|
ring_capacity);
|
||||||
|
if (!registered && !hub.hasSource(track_id)) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register camera track: " << track_id;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ensureMicrophoneMediaSource(
|
||||||
|
MediaSourceHub& hub,
|
||||||
|
const std::shared_ptr<device::AbstractMicrophone>& microphone,
|
||||||
|
const size_t ring_capacity) {
|
||||||
|
if (!microphone || microphone->id().empty()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const std::string track_id = microphoneTrackId(microphone->id());
|
||||||
|
if (hub.hasSource(track_id)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto pump = std::make_shared<MicrophonePump>(microphone, track_id);
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [pump](
|
||||||
|
const MediaSourceHub::FrameSink& sink,
|
||||||
|
const MediaSourceHub::CancelPredicate& cancelled) {
|
||||||
|
return pump->begin(sink, cancelled);
|
||||||
|
};
|
||||||
|
callbacks.stop = [pump] { pump->stop(); };
|
||||||
|
const bool registered = hub.registerSource(
|
||||||
|
initialTrack(track_id, microphone->id(), MediaKind::AUDIO),
|
||||||
|
std::move(callbacks),
|
||||||
|
ring_capacity);
|
||||||
|
if (!registered && !hub.hasSource(track_id)) {
|
||||||
|
CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register microphone track: " << track_id;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::media
|
||||||
596
cmvr-es/manager/media_source_hub/src/media_source_hub.cpp
Normal file
596
cmvr-es/manager/media_source_hub/src/media_source_hub.cpp
Normal file
@ -0,0 +1,596 @@
|
|||||||
|
#include "manager/media_source_hub/include/media_source_hub.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <atomic>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <mutex>
|
||||||
|
#include <thread>
|
||||||
|
#include <unordered_map>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
|
namespace cmvr::media {
|
||||||
|
|
||||||
|
struct MediaSourceHub::SourceState final : public std::enable_shared_from_this<SourceState> {
|
||||||
|
enum class Lifecycle {
|
||||||
|
STOPPED,
|
||||||
|
STARTING,
|
||||||
|
RUNNING,
|
||||||
|
STOPPING
|
||||||
|
};
|
||||||
|
|
||||||
|
struct StartAttempt {
|
||||||
|
size_t waiters{0};
|
||||||
|
bool completed{false};
|
||||||
|
bool succeeded{false};
|
||||||
|
std::atomic<bool> cancel_requested{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
SourceState(
|
||||||
|
TrackDescriptorPtr initial_descriptor,
|
||||||
|
SourceCallbacks source_callbacks,
|
||||||
|
const size_t ring_capacity)
|
||||||
|
: track_id(initial_descriptor->id),
|
||||||
|
descriptor(std::move(initial_descriptor)),
|
||||||
|
callbacks(std::move(source_callbacks)),
|
||||||
|
ring(ring_capacity) {}
|
||||||
|
|
||||||
|
FrameSink makeSink() {
|
||||||
|
const std::weak_ptr<SourceState> weak_source = shared_from_this();
|
||||||
|
return [weak_source](MediaFramePtr frame) {
|
||||||
|
if (const auto source = weak_source.lock()) {
|
||||||
|
source->acceptFrame(std::move(frame));
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
void acceptFrame(MediaFramePtr frame) {
|
||||||
|
if (!frame || !frame->descriptor || frame->descriptor->id != track_id) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (!registered ||
|
||||||
|
(lifecycle != Lifecycle::STARTING && lifecycle != Lifecycle::RUNNING)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const TrackDescriptorPtr current = std::atomic_load(&descriptor);
|
||||||
|
if (!current || !equivalentTrackDescriptor(*current, *frame->descriptor)) {
|
||||||
|
// C++17 atomic shared_ptr free functions provide an atomic descriptor snapshot to
|
||||||
|
// all subscriptions while frames remain immutable.
|
||||||
|
std::atomic_store(&descriptor, frame->descriptor);
|
||||||
|
}
|
||||||
|
ring.publish(std::move(frame));
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool isCancelled(const CancelPredicate& cancelled) noexcept {
|
||||||
|
if (!cancelled) return false;
|
||||||
|
try {
|
||||||
|
return cancelled();
|
||||||
|
} catch (...) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool invokeStart(
|
||||||
|
const FrameSink& sink,
|
||||||
|
const CancelPredicate& cancelled) noexcept {
|
||||||
|
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||||
|
try {
|
||||||
|
return callbacks.start && callbacks.start(sink, cancelled);
|
||||||
|
} catch (...) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void invokeStop() noexcept {
|
||||||
|
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||||
|
try {
|
||||||
|
if (callbacks.stop) callbacks.stop();
|
||||||
|
} catch (...) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void completeStart(
|
||||||
|
const std::shared_ptr<StartAttempt>& attempt,
|
||||||
|
const bool started) {
|
||||||
|
bool stop_abandoned_start = false;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (start_attempt != attempt || lifecycle != Lifecycle::STARTING) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
attempt->completed = true;
|
||||||
|
attempt->succeeded = started;
|
||||||
|
if (started && registered && attempt->waiters != 0U) {
|
||||||
|
lifecycle = Lifecycle::RUNNING;
|
||||||
|
} else if (started) {
|
||||||
|
lifecycle = Lifecycle::STOPPING;
|
||||||
|
ring.close();
|
||||||
|
stop_abandoned_start = true;
|
||||||
|
} else {
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (stop_abandoned_start) {
|
||||||
|
invokeStop();
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool acquire(
|
||||||
|
const StartPosition start_position,
|
||||||
|
FrameRing::Cursor& cursor,
|
||||||
|
const CancelPredicate& cancelled) {
|
||||||
|
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||||
|
while (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
if (!registered || isCancelled(cancelled)) return false;
|
||||||
|
lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10));
|
||||||
|
}
|
||||||
|
if (!registered || isCancelled(cancelled)) return false;
|
||||||
|
|
||||||
|
if (lifecycle == Lifecycle::RUNNING) {
|
||||||
|
++subscriber_count;
|
||||||
|
lock.unlock();
|
||||||
|
cursor = ring.makeCursor(start_position);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<StartAttempt> attempt;
|
||||||
|
if (lifecycle == Lifecycle::STOPPED) {
|
||||||
|
lifecycle = Lifecycle::STARTING;
|
||||||
|
ring.reset();
|
||||||
|
const FrameSink sink = makeSink();
|
||||||
|
attempt = std::make_shared<StartAttempt>();
|
||||||
|
attempt->waiters = 1U;
|
||||||
|
start_attempt = attempt;
|
||||||
|
const auto self = shared_from_this();
|
||||||
|
try {
|
||||||
|
std::thread([self, attempt, sink]() {
|
||||||
|
const CancelPredicate cancelled = [attempt] {
|
||||||
|
return attempt->cancel_requested.load(
|
||||||
|
std::memory_order_acquire);
|
||||||
|
};
|
||||||
|
const bool started = self->invokeStart(sink, cancelled);
|
||||||
|
self->completeStart(attempt, started);
|
||||||
|
}).detach();
|
||||||
|
} catch (...) {
|
||||||
|
start_attempt.reset();
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
} else if (lifecycle == Lifecycle::STARTING) {
|
||||||
|
attempt = start_attempt;
|
||||||
|
if (!attempt || attempt->cancel_requested.load(std::memory_order_acquire)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
++attempt->waiters;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
while (registered && !attempt->completed) {
|
||||||
|
if (isCancelled(cancelled)) {
|
||||||
|
if (attempt->waiters != 0U) --attempt->waiters;
|
||||||
|
if (attempt->waiters == 0U) {
|
||||||
|
attempt->cancel_requested.store(true, std::memory_order_release);
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10));
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool caller_cancelled = isCancelled(cancelled);
|
||||||
|
const bool acquired = !caller_cancelled && registered && attempt->completed &&
|
||||||
|
attempt->succeeded &&
|
||||||
|
lifecycle == Lifecycle::RUNNING;
|
||||||
|
if (attempt->waiters != 0U) --attempt->waiters;
|
||||||
|
if (!acquired) {
|
||||||
|
if (attempt->waiters == 0U) {
|
||||||
|
attempt->cancel_requested.store(true, std::memory_order_release);
|
||||||
|
}
|
||||||
|
const bool stop_unclaimed_source =
|
||||||
|
start_attempt == attempt && attempt->waiters == 0U &&
|
||||||
|
subscriber_count == 0U &&
|
||||||
|
lifecycle == Lifecycle::RUNNING;
|
||||||
|
if (stop_unclaimed_source) {
|
||||||
|
lifecycle = Lifecycle::STOPPING;
|
||||||
|
ring.close();
|
||||||
|
lock.unlock();
|
||||||
|
invokeStop();
|
||||||
|
lock.lock();
|
||||||
|
if (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
++subscriber_count;
|
||||||
|
lock.unlock();
|
||||||
|
cursor = ring.makeCursor(start_position);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void release() {
|
||||||
|
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (subscriber_count == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
--subscriber_count;
|
||||||
|
if (subscriber_count != 0 || lifecycle != Lifecycle::RUNNING) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
lifecycle = Lifecycle::STOPPING;
|
||||||
|
ring.close();
|
||||||
|
lock.unlock();
|
||||||
|
invokeStop();
|
||||||
|
lock.lock();
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool deactivateIfUnused() {
|
||||||
|
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (subscriber_count != 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
registered = false;
|
||||||
|
ring.close();
|
||||||
|
if (lifecycle == Lifecycle::STARTING) {
|
||||||
|
if (start_attempt) {
|
||||||
|
start_attempt->cancel_requested.store(true, std::memory_order_release);
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (lifecycle == Lifecycle::STOPPING || lifecycle == Lifecycle::STOPPED) {
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
lifecycle = Lifecycle::STOPPING;
|
||||||
|
lock.unlock();
|
||||||
|
invokeStop();
|
||||||
|
lock.lock();
|
||||||
|
if (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void shutdown() {
|
||||||
|
std::unique_lock<std::mutex> lock(lifecycle_mutex);
|
||||||
|
registered = false;
|
||||||
|
ring.close();
|
||||||
|
if (lifecycle == Lifecycle::STARTING) {
|
||||||
|
if (start_attempt) {
|
||||||
|
start_attempt->cancel_requested.store(true, std::memory_order_release);
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (lifecycle == Lifecycle::STOPPED) {
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
lifecycle = Lifecycle::STOPPING;
|
||||||
|
lock.unlock();
|
||||||
|
invokeStop();
|
||||||
|
lock.lock();
|
||||||
|
if (lifecycle == Lifecycle::STOPPING) {
|
||||||
|
lifecycle = Lifecycle::STOPPED;
|
||||||
|
}
|
||||||
|
lifecycle_condition.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool validForSubscription() const {
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
return registered && lifecycle == Lifecycle::RUNNING;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t subscriberCount() const {
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
return subscriber_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool requestKeyFrame() const {
|
||||||
|
// Serialize with stop first, then re-check lifecycle. A stop that has
|
||||||
|
// already begun rejects the request; a stop that begins afterwards
|
||||||
|
// waits for this callback before invoking the device stop barrier.
|
||||||
|
std::lock_guard<std::mutex> callback_lock(callback_mutex);
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(lifecycle_mutex);
|
||||||
|
if (!registered || lifecycle != Lifecycle::RUNNING || !callbacks.request_key_frame) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
return callbacks.request_key_frame();
|
||||||
|
} catch (...) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TrackDescriptorPtr currentDescriptor() const {
|
||||||
|
return std::atomic_load(&descriptor);
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string track_id;
|
||||||
|
mutable TrackDescriptorPtr descriptor;
|
||||||
|
const SourceCallbacks callbacks;
|
||||||
|
FrameRing ring;
|
||||||
|
|
||||||
|
mutable std::mutex callback_mutex;
|
||||||
|
mutable std::mutex lifecycle_mutex;
|
||||||
|
std::condition_variable lifecycle_condition;
|
||||||
|
Lifecycle lifecycle{Lifecycle::STOPPED};
|
||||||
|
size_t subscriber_count{0};
|
||||||
|
bool registered{true};
|
||||||
|
std::shared_ptr<StartAttempt> start_attempt;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct MediaSourceHub::Impl final {
|
||||||
|
mutable std::mutex mutex;
|
||||||
|
std::unordered_map<std::string, std::shared_ptr<SourceState>> sources;
|
||||||
|
};
|
||||||
|
|
||||||
|
MediaSourceHub::Subscription::Subscription(
|
||||||
|
std::shared_ptr<SourceState> source,
|
||||||
|
FrameRing::Cursor cursor)
|
||||||
|
: source_(std::move(source)),
|
||||||
|
cursor_(std::move(cursor)),
|
||||||
|
active_(static_cast<bool>(source_)) {}
|
||||||
|
|
||||||
|
MediaSourceHub::Subscription::~Subscription() {
|
||||||
|
reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept
|
||||||
|
: source_(std::move(other.source_)),
|
||||||
|
cursor_(other.cursor_),
|
||||||
|
active_(other.active_) {
|
||||||
|
other.active_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscription&& other) noexcept {
|
||||||
|
if (this == &other) {
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
reset();
|
||||||
|
source_ = std::move(other.source_);
|
||||||
|
cursor_ = other.cursor_;
|
||||||
|
active_ = other.active_;
|
||||||
|
other.active_ = false;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MediaSourceHub::Subscription::valid() const {
|
||||||
|
return active_ && source_ && source_->validForSubscription();
|
||||||
|
}
|
||||||
|
|
||||||
|
TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const {
|
||||||
|
return source_ ? source_->currentDescriptor() : nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::tryRead() {
|
||||||
|
if (!active_ || !source_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return source_->ring.tryRead(cursor_);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<MediaSourceHub::FrameReadResult> MediaSourceHub::Subscription::waitRead(
|
||||||
|
const std::chrono::milliseconds timeout) {
|
||||||
|
if (!active_ || !source_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return source_->ring.waitRead(cursor_, timeout);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds(
|
||||||
|
const size_t maximum_pending_frames) {
|
||||||
|
if (!active_ || !source_) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept {
|
||||||
|
return cursor_.dropped_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MediaSourceHub::Subscription::reset() {
|
||||||
|
if (active_ && source_) {
|
||||||
|
source_->release();
|
||||||
|
}
|
||||||
|
active_ = false;
|
||||||
|
source_.reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
MediaSourceHub::MediaSourceHub()
|
||||||
|
: impl_(std::make_shared<Impl>()) {}
|
||||||
|
|
||||||
|
MediaSourceHub::~MediaSourceHub() {
|
||||||
|
shutdown();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MediaSourceHub::registerSource(
|
||||||
|
TrackDescriptorPtr initial_descriptor,
|
||||||
|
SourceCallbacks callbacks,
|
||||||
|
const size_t ring_capacity) {
|
||||||
|
if (!impl_ || !initial_descriptor || initial_descriptor->id.empty() ||
|
||||||
|
!callbacks.start || ring_capacity == 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<SourceState> source;
|
||||||
|
try {
|
||||||
|
source = std::make_shared<SourceState>(
|
||||||
|
std::move(initial_descriptor), std::move(callbacks), ring_capacity);
|
||||||
|
} catch (...) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
return impl_->sources.emplace(source->track_id, std::move(source)).second;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MediaSourceHub::unregisterSource(const std::string& track_id) {
|
||||||
|
if (!impl_ || track_id.empty()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<SourceState> source;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
const auto it = impl_->sources.find(track_id);
|
||||||
|
if (it == impl_->sources.end()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
source = it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!source->deactivateIfUnused()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
const auto it = impl_->sources.find(track_id);
|
||||||
|
if (it != impl_->sources.end() && it->second == source) {
|
||||||
|
impl_->sources.erase(it);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MediaSourceHub::hasSource(const std::string& track_id) const {
|
||||||
|
if (!impl_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
return impl_->sources.find(track_id) != impl_->sources.end();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<TrackDescriptorPtr> MediaSourceHub::listTracks() const {
|
||||||
|
std::vector<std::shared_ptr<SourceState>> sources;
|
||||||
|
if (!impl_) {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
sources.reserve(impl_->sources.size());
|
||||||
|
for (const auto& [track_id, source] : impl_->sources) {
|
||||||
|
(void)track_id;
|
||||||
|
sources.push_back(source);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<TrackDescriptorPtr> descriptors;
|
||||||
|
descriptors.reserve(sources.size());
|
||||||
|
for (const auto& source : sources) {
|
||||||
|
descriptors.push_back(source->currentDescriptor());
|
||||||
|
}
|
||||||
|
std::sort(descriptors.begin(), descriptors.end(), [](const auto& lhs, const auto& rhs) {
|
||||||
|
if (!lhs) return static_cast<bool>(rhs);
|
||||||
|
if (!rhs) return false;
|
||||||
|
return lhs->id < rhs->id;
|
||||||
|
});
|
||||||
|
return descriptors;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t MediaSourceHub::subscriberCount(const std::string& track_id) const {
|
||||||
|
if (!impl_) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
std::shared_ptr<SourceState> source;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
const auto it = impl_->sources.find(track_id);
|
||||||
|
if (it == impl_->sources.end()) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
source = it->second;
|
||||||
|
}
|
||||||
|
return source->subscriberCount();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const {
|
||||||
|
if (!impl_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
std::shared_ptr<SourceState> source;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
const auto it = impl_->sources.find(track_id);
|
||||||
|
if (it == impl_->sources.end()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
source = it->second;
|
||||||
|
}
|
||||||
|
return source->requestKeyFrame();
|
||||||
|
}
|
||||||
|
|
||||||
|
MediaSourceHub::Subscription MediaSourceHub::subscribe(
|
||||||
|
const std::string& track_id,
|
||||||
|
const StartPosition start_position,
|
||||||
|
CancelPredicate cancelled) {
|
||||||
|
if (!impl_) {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<SourceState> source;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
const auto it = impl_->sources.find(track_id);
|
||||||
|
if (it == impl_->sources.end()) {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
source = it->second;
|
||||||
|
}
|
||||||
|
|
||||||
|
FrameRing::Cursor cursor;
|
||||||
|
if (!source->acquire(start_position, cursor, cancelled)) {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
return Subscription(std::move(source), std::move(cursor));
|
||||||
|
}
|
||||||
|
|
||||||
|
void MediaSourceHub::shutdown() {
|
||||||
|
if (!impl_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::shared_ptr<SourceState>> sources;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||||
|
sources.reserve(impl_->sources.size());
|
||||||
|
for (auto& [track_id, source] : impl_->sources) {
|
||||||
|
(void)track_id;
|
||||||
|
sources.push_back(std::move(source));
|
||||||
|
}
|
||||||
|
impl_->sources.clear();
|
||||||
|
}
|
||||||
|
for (const auto& source : sources) {
|
||||||
|
source->shutdown();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::media
|
||||||
683
cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp
Normal file
683
cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp
Normal file
@ -0,0 +1,683 @@
|
|||||||
|
#include "manager/media_source_hub/include/media_source_hub.h"
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <future>
|
||||||
|
#include <iostream>
|
||||||
|
#include <mutex>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <type_traits>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
using cmvr::media::Codec;
|
||||||
|
using cmvr::media::MediaFrame;
|
||||||
|
using cmvr::media::MediaFramePtr;
|
||||||
|
using cmvr::media::MediaKind;
|
||||||
|
using cmvr::media::MediaSourceHub;
|
||||||
|
using cmvr::media::PayloadFormat;
|
||||||
|
using cmvr::media::Rational;
|
||||||
|
using cmvr::media::TrackDescriptor;
|
||||||
|
using cmvr::media::TrackDescriptorPtr;
|
||||||
|
|
||||||
|
static_assert(!std::is_copy_assignable<MediaFrame>::value, "MediaFrame must be immutable");
|
||||||
|
static_assert(!std::is_copy_assignable<TrackDescriptor>::value, "TrackDescriptor must be immutable");
|
||||||
|
static_assert(std::is_same<MediaFramePtr::element_type, const MediaFrame>::value,
|
||||||
|
"MediaFramePtr must share const frames");
|
||||||
|
|
||||||
|
int failures = 0;
|
||||||
|
|
||||||
|
#define CHECK_TRUE(expression) \
|
||||||
|
do { \
|
||||||
|
if (!(expression)) { \
|
||||||
|
std::cerr << __FILE__ << ':' << __LINE__ << " check failed: " #expression << '\n'; \
|
||||||
|
++failures; \
|
||||||
|
} \
|
||||||
|
} while (false)
|
||||||
|
|
||||||
|
TrackDescriptorPtr makeVideoDescriptor(
|
||||||
|
const Codec codec,
|
||||||
|
const uint64_t generation,
|
||||||
|
std::vector<uint8_t> codec_config = {}) {
|
||||||
|
TrackDescriptor::Config config;
|
||||||
|
config.id = "camera.front.video";
|
||||||
|
config.source_id = "camera.front";
|
||||||
|
config.kind = MediaKind::VIDEO;
|
||||||
|
config.codec = codec;
|
||||||
|
config.payload_format = codec == Codec::UNKNOWN ? PayloadFormat::UNKNOWN : PayloadFormat::ANNEX_B;
|
||||||
|
config.time_base = Rational{1, 90000};
|
||||||
|
config.width = 640;
|
||||||
|
config.height = 360;
|
||||||
|
config.nominal_rate = 30;
|
||||||
|
config.generation = generation;
|
||||||
|
config.codec_config = std::move(codec_config);
|
||||||
|
return cmvr::media::makeTrackDescriptor(std::move(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
MediaFramePtr makeFrame(
|
||||||
|
TrackDescriptorPtr descriptor,
|
||||||
|
const uint64_t sequence,
|
||||||
|
const uint8_t marker) {
|
||||||
|
MediaFrame::Config config;
|
||||||
|
config.descriptor = std::move(descriptor);
|
||||||
|
config.payload = {marker, static_cast<uint8_t>(marker + 1)};
|
||||||
|
config.sequence = sequence;
|
||||||
|
config.pts = static_cast<int64_t>(sequence * 3000);
|
||||||
|
config.dts = config.pts;
|
||||||
|
config.duration = 3000;
|
||||||
|
config.capture_time_ns = sequence * 1000000;
|
||||||
|
config.capture_utc_ns = 1700000000000000000LL + static_cast<int64_t>(sequence);
|
||||||
|
config.key_frame = sequence == 0;
|
||||||
|
return cmvr::media::makeMediaFrame(std::move(config));
|
||||||
|
}
|
||||||
|
|
||||||
|
void testMediaMetadataValidation() {
|
||||||
|
TrackDescriptor::Config invalid;
|
||||||
|
invalid.id = "invalid.video";
|
||||||
|
invalid.source_id = "invalid";
|
||||||
|
invalid.kind = MediaKind::VIDEO;
|
||||||
|
invalid.time_base = Rational{0, 1};
|
||||||
|
bool rejected = false;
|
||||||
|
try {
|
||||||
|
(void)cmvr::media::makeTrackDescriptor(std::move(invalid));
|
||||||
|
} catch (const std::invalid_argument&) {
|
||||||
|
rejected = true;
|
||||||
|
}
|
||||||
|
CHECK_TRUE(rejected);
|
||||||
|
|
||||||
|
const auto frame = makeFrame(makeVideoDescriptor(Codec::H264, 1), 7, 1);
|
||||||
|
CHECK_TRUE(frame->capture_time_ns == 7000000);
|
||||||
|
CHECK_TRUE(frame->capture_utc_ns == 1700000000000000007LL);
|
||||||
|
CHECK_TRUE(frame->duration == 3000);
|
||||||
|
}
|
||||||
|
|
||||||
|
void testLegacySpmcCompatibility() {
|
||||||
|
bool ring_zero_capacity_rejected = false;
|
||||||
|
try {
|
||||||
|
RingBuffer<int> invalid_ring(0);
|
||||||
|
} catch (const std::invalid_argument&) {
|
||||||
|
ring_zero_capacity_rejected = true;
|
||||||
|
}
|
||||||
|
CHECK_TRUE(ring_zero_capacity_rejected);
|
||||||
|
|
||||||
|
bool spmc_zero_capacity_rejected = false;
|
||||||
|
try {
|
||||||
|
SPMCRingBuffer<int> invalid_ring(0);
|
||||||
|
} catch (const std::invalid_argument&) {
|
||||||
|
spmc_zero_capacity_rejected = true;
|
||||||
|
}
|
||||||
|
CHECK_TRUE(spmc_zero_capacity_rejected);
|
||||||
|
|
||||||
|
SPMCRingBuffer<int> ring(2);
|
||||||
|
ring.push(10);
|
||||||
|
ring.push(20);
|
||||||
|
CHECK_TRUE(ring.size() == 2);
|
||||||
|
CHECK_TRUE(ring.getHead() == 2);
|
||||||
|
CHECK_TRUE(ring.getTail() == 0);
|
||||||
|
CHECK_TRUE(ring.getLast().has_value() && *ring.getLast() == 20);
|
||||||
|
|
||||||
|
size_t reader = 0;
|
||||||
|
CHECK_TRUE(ring.pop(reader).has_value());
|
||||||
|
ring.push(30);
|
||||||
|
ring.push(40);
|
||||||
|
CHECK_TRUE(!ring.pop(reader).has_value());
|
||||||
|
CHECK_TRUE(reader == ring.getTail());
|
||||||
|
CHECK_TRUE(ring.pop(reader).has_value());
|
||||||
|
|
||||||
|
ring.clear();
|
||||||
|
CHECK_TRUE(ring.empty());
|
||||||
|
CHECK_TRUE(ring.getHead() == 4);
|
||||||
|
ring.push(50);
|
||||||
|
CHECK_TRUE(!ring.pop(reader).has_value());
|
||||||
|
const auto after_clear = ring.pop(reader);
|
||||||
|
CHECK_TRUE(after_clear.has_value() && *after_clear == 50);
|
||||||
|
}
|
||||||
|
|
||||||
|
void testBroadcastFrameRing() {
|
||||||
|
using Ring = BroadcastFrameRing<MediaFrame>;
|
||||||
|
Ring ring(2);
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
|
||||||
|
CHECK_TRUE(ring.publish(makeFrame(descriptor, 0, 10)).value() == 0);
|
||||||
|
CHECK_TRUE(ring.publish(makeFrame(descriptor, 1, 20)).value() == 1);
|
||||||
|
CHECK_TRUE(ring.publish(makeFrame(descriptor, 2, 30)).value() == 2);
|
||||||
|
|
||||||
|
const auto first = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(first.has_value());
|
||||||
|
CHECK_TRUE(first->sequence == 1);
|
||||||
|
CHECK_TRUE(first->value->sequence == 1);
|
||||||
|
CHECK_TRUE(first->dropped_count == 1);
|
||||||
|
CHECK_TRUE(first->dropped_since_last_read == 1);
|
||||||
|
CHECK_TRUE(ring.stats().dropped_count == 1);
|
||||||
|
|
||||||
|
const auto second = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(second.has_value() && second->sequence == 2);
|
||||||
|
CHECK_TRUE(second->dropped_since_last_read == 0);
|
||||||
|
|
||||||
|
auto waiting_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
|
||||||
|
auto waiting_read = std::async(std::launch::async, [&ring, &waiting_cursor] {
|
||||||
|
return ring.waitRead(waiting_cursor, 1s);
|
||||||
|
});
|
||||||
|
std::this_thread::sleep_for(10ms);
|
||||||
|
ring.publish(makeFrame(descriptor, 3, 40));
|
||||||
|
CHECK_TRUE(waiting_read.wait_for(500ms) == std::future_status::ready);
|
||||||
|
CHECK_TRUE(waiting_read.get().has_value());
|
||||||
|
|
||||||
|
const uint64_t next_generation = ring.reset();
|
||||||
|
CHECK_TRUE(next_generation == 2);
|
||||||
|
ring.publish(makeFrame(descriptor, 4, 50));
|
||||||
|
const auto after_reset = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(after_reset.has_value());
|
||||||
|
CHECK_TRUE(after_reset->generation == 2);
|
||||||
|
CHECK_TRUE(after_reset->sequence == 0);
|
||||||
|
CHECK_TRUE(after_reset->generation_changed);
|
||||||
|
|
||||||
|
auto close_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
|
||||||
|
auto close_wait = std::async(std::launch::async, [&ring, &close_cursor] {
|
||||||
|
return ring.waitRead(close_cursor, 2s);
|
||||||
|
});
|
||||||
|
ring.close();
|
||||||
|
CHECK_TRUE(close_wait.wait_for(500ms) == std::future_status::ready);
|
||||||
|
CHECK_TRUE(!close_wait.get().has_value());
|
||||||
|
CHECK_TRUE(!ring.publish(makeFrame(descriptor, 5, 60)).has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
void testBroadcastDiscardPending() {
|
||||||
|
using Ring = BroadcastFrameRing<MediaFrame>;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
|
||||||
|
|
||||||
|
{
|
||||||
|
Ring ring(8);
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
ring.publish(makeFrame(descriptor, 0, 10));
|
||||||
|
ring.publish(makeFrame(descriptor, 1, 20));
|
||||||
|
|
||||||
|
CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 0);
|
||||||
|
const auto first = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(first.has_value());
|
||||||
|
CHECK_TRUE(first->sequence == 0);
|
||||||
|
CHECK_TRUE(first->dropped_since_last_read == 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
Ring ring(8);
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
ring.publish(makeFrame(descriptor, 0, 10));
|
||||||
|
ring.publish(makeFrame(descriptor, 1, 20));
|
||||||
|
ring.publish(makeFrame(descriptor, 2, 30));
|
||||||
|
|
||||||
|
CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3);
|
||||||
|
CHECK_TRUE(!ring.tryRead(cursor).has_value());
|
||||||
|
|
||||||
|
ring.publish(makeFrame(descriptor, 3, 40));
|
||||||
|
const auto after_discard = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(after_discard.has_value());
|
||||||
|
CHECK_TRUE(after_discard->sequence == 3);
|
||||||
|
CHECK_TRUE(after_discard->dropped_count == 3);
|
||||||
|
CHECK_TRUE(after_discard->dropped_since_last_read == 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
// Two frames are overwritten before the explicit three-frame discard.
|
||||||
|
// Both kinds of loss must be reported by the next successful read.
|
||||||
|
Ring ring(3);
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
for (uint64_t sequence = 0; sequence < 5; ++sequence) {
|
||||||
|
ring.publish(makeFrame(
|
||||||
|
descriptor,
|
||||||
|
sequence,
|
||||||
|
static_cast<uint8_t>(sequence)));
|
||||||
|
}
|
||||||
|
|
||||||
|
CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3);
|
||||||
|
CHECK_TRUE(cursor.dropped_count == 5);
|
||||||
|
ring.publish(makeFrame(descriptor, 5, 50));
|
||||||
|
const auto after_overwrite_and_discard = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(after_overwrite_and_discard.has_value());
|
||||||
|
CHECK_TRUE(after_overwrite_and_discard->sequence == 5);
|
||||||
|
CHECK_TRUE(after_overwrite_and_discard->dropped_count == 5);
|
||||||
|
CHECK_TRUE(after_overwrite_and_discard->dropped_since_last_read == 5);
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
// An old-generation OLDEST_AVAILABLE cursor adopts the reset generation
|
||||||
|
// before deciding whether that generation's pending frames are excessive.
|
||||||
|
Ring ring(4);
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
ring.publish(makeFrame(descriptor, 0, 10));
|
||||||
|
ring.reset();
|
||||||
|
ring.publish(makeFrame(descriptor, 1, 20));
|
||||||
|
ring.publish(makeFrame(descriptor, 2, 30));
|
||||||
|
|
||||||
|
CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 1) == 2);
|
||||||
|
ring.publish(makeFrame(descriptor, 3, 40));
|
||||||
|
const auto after_reset = ring.tryRead(cursor);
|
||||||
|
CHECK_TRUE(after_reset.has_value());
|
||||||
|
CHECK_TRUE(after_reset->generation == 2);
|
||||||
|
CHECK_TRUE(after_reset->sequence == 2);
|
||||||
|
CHECK_TRUE(after_reset->generation_changed);
|
||||||
|
CHECK_TRUE(after_reset->dropped_since_last_read == 2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void testBroadcastDiscardConcurrentPublish() {
|
||||||
|
using Ring = BroadcastFrameRing<int>;
|
||||||
|
constexpr uint64_t frame_count = 4000;
|
||||||
|
Ring ring(64);
|
||||||
|
auto cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED);
|
||||||
|
std::atomic<bool> start{false};
|
||||||
|
std::atomic<bool> publisher_done{false};
|
||||||
|
|
||||||
|
std::thread publisher([&] {
|
||||||
|
while (!start.load(std::memory_order_acquire)) {
|
||||||
|
std::this_thread::yield();
|
||||||
|
}
|
||||||
|
for (uint64_t sequence = 0; sequence < frame_count; ++sequence) {
|
||||||
|
ring.publish(std::make_shared<const int>(static_cast<int>(sequence)));
|
||||||
|
if ((sequence & 7U) == 0U) {
|
||||||
|
std::this_thread::yield();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
publisher_done.store(true, std::memory_order_release);
|
||||||
|
});
|
||||||
|
|
||||||
|
uint64_t read_count = 0;
|
||||||
|
uint64_t actively_discarded = 0;
|
||||||
|
start.store(true, std::memory_order_release);
|
||||||
|
while (true) {
|
||||||
|
actively_discarded += ring.discardPendingIfExceeds(cursor, 8);
|
||||||
|
if (ring.tryRead(cursor)) {
|
||||||
|
++read_count;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (publisher_done.load(std::memory_order_acquire)) {
|
||||||
|
actively_discarded += ring.discardPendingIfExceeds(cursor, 8);
|
||||||
|
if (ring.tryRead(cursor)) {
|
||||||
|
++read_count;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
std::this_thread::yield();
|
||||||
|
}
|
||||||
|
publisher.join();
|
||||||
|
|
||||||
|
CHECK_TRUE(read_count + cursor.dropped_count == frame_count);
|
||||||
|
CHECK_TRUE(actively_discarded <= cursor.dropped_count);
|
||||||
|
}
|
||||||
|
|
||||||
|
void testBroadcastConcurrency() {
|
||||||
|
using Ring = BroadcastFrameRing<MediaFrame>;
|
||||||
|
constexpr uint64_t frame_count = 500;
|
||||||
|
Ring ring(frame_count);
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3});
|
||||||
|
auto first_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
auto second_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE);
|
||||||
|
|
||||||
|
auto consume = [&ring](Ring::Cursor& cursor) {
|
||||||
|
uint64_t expected = 0;
|
||||||
|
while (expected < frame_count) {
|
||||||
|
const auto result = ring.waitRead(cursor, 1s);
|
||||||
|
if (!result || result->sequence != expected || result->value->sequence != expected) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
++expected;
|
||||||
|
}
|
||||||
|
return cursor.dropped_count == 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
auto first_consumer = std::async(std::launch::async, consume, std::ref(first_cursor));
|
||||||
|
auto second_consumer = std::async(std::launch::async, consume, std::ref(second_cursor));
|
||||||
|
std::thread producer([&ring, &descriptor] {
|
||||||
|
for (uint64_t sequence = 0; sequence < frame_count; ++sequence) {
|
||||||
|
ring.publish(makeFrame(descriptor, sequence, static_cast<uint8_t>(sequence)));
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
producer.join();
|
||||||
|
CHECK_TRUE(first_consumer.get());
|
||||||
|
CHECK_TRUE(second_consumer.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
void testHubLifecycleAndDescriptorRefresh() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto initial_descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
|
||||||
|
std::atomic<int> start_count{0};
|
||||||
|
std::atomic<int> stop_count{0};
|
||||||
|
std::atomic<int> key_frame_requests{0};
|
||||||
|
std::mutex sink_mutex;
|
||||||
|
MediaSourceHub::FrameSink sink;
|
||||||
|
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
sink = callback_sink;
|
||||||
|
}
|
||||||
|
++start_count;
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
callbacks.stop = [&] {
|
||||||
|
++stop_count;
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
sink = {};
|
||||||
|
};
|
||||||
|
callbacks.request_key_frame = [&] {
|
||||||
|
++key_frame_requests;
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
|
||||||
|
CHECK_TRUE(hub.registerSource(initial_descriptor, callbacks, 4));
|
||||||
|
CHECK_TRUE(!hub.registerSource(initial_descriptor, callbacks, 4));
|
||||||
|
CHECK_TRUE(hub.hasSource(initial_descriptor->id));
|
||||||
|
CHECK_TRUE(hub.listTracks().size() == 1);
|
||||||
|
|
||||||
|
auto first = hub.subscribe(initial_descriptor->id);
|
||||||
|
auto second = hub.subscribe(initial_descriptor->id);
|
||||||
|
CHECK_TRUE(first.valid() && second.valid());
|
||||||
|
CHECK_TRUE(hub.requestKeyFrame(initial_descriptor->id));
|
||||||
|
CHECK_TRUE(key_frame_requests == 1);
|
||||||
|
CHECK_TRUE(start_count == 1);
|
||||||
|
CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 2);
|
||||||
|
CHECK_TRUE(first.descriptor()->codec == Codec::UNKNOWN);
|
||||||
|
CHECK_TRUE(!hub.unregisterSource(initial_descriptor->id));
|
||||||
|
|
||||||
|
// Content changes at the same generation must atomically replace the initial descriptor.
|
||||||
|
const auto actual_descriptor = makeVideoDescriptor(Codec::H264, 1, {0, 0, 0, 1, 0x67});
|
||||||
|
MediaSourceHub::FrameSink producer;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
producer = sink;
|
||||||
|
}
|
||||||
|
CHECK_TRUE(static_cast<bool>(producer));
|
||||||
|
const auto shared_frame = makeFrame(actual_descriptor, 0, 70);
|
||||||
|
producer(shared_frame);
|
||||||
|
|
||||||
|
const auto first_read = first.waitRead(500ms);
|
||||||
|
const auto second_read = second.waitRead(500ms);
|
||||||
|
CHECK_TRUE(first_read.has_value() && first_read->value == shared_frame);
|
||||||
|
CHECK_TRUE(second_read.has_value() && second_read->value == shared_frame);
|
||||||
|
CHECK_TRUE(first.descriptor() == actual_descriptor);
|
||||||
|
CHECK_TRUE(second.descriptor()->codec_config == actual_descriptor->codec_config);
|
||||||
|
|
||||||
|
first.reset();
|
||||||
|
CHECK_TRUE(stop_count == 0);
|
||||||
|
CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 1);
|
||||||
|
second.reset();
|
||||||
|
CHECK_TRUE(stop_count == 1);
|
||||||
|
CHECK_TRUE(!hub.requestKeyFrame(initial_descriptor->id));
|
||||||
|
CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 0);
|
||||||
|
|
||||||
|
{
|
||||||
|
auto restarted = hub.subscribe(initial_descriptor->id);
|
||||||
|
CHECK_TRUE(restarted.valid());
|
||||||
|
CHECK_TRUE(start_count == 2);
|
||||||
|
}
|
||||||
|
CHECK_TRUE(stop_count == 2);
|
||||||
|
CHECK_TRUE(hub.unregisterSource(initial_descriptor->id));
|
||||||
|
CHECK_TRUE(!hub.hasSource(initial_descriptor->id));
|
||||||
|
}
|
||||||
|
|
||||||
|
void testSubscriptionDiscardPending() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
|
||||||
|
std::mutex sink_mutex;
|
||||||
|
MediaSourceHub::FrameSink sink;
|
||||||
|
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
sink = callback_sink;
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
callbacks.stop = [&] {
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
sink = {};
|
||||||
|
};
|
||||||
|
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8));
|
||||||
|
auto subscription = hub.subscribe(descriptor->id);
|
||||||
|
CHECK_TRUE(subscription.valid());
|
||||||
|
|
||||||
|
MediaSourceHub::FrameSink producer;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sink_mutex);
|
||||||
|
producer = sink;
|
||||||
|
}
|
||||||
|
CHECK_TRUE(static_cast<bool>(producer));
|
||||||
|
producer(makeFrame(descriptor, 0, 10));
|
||||||
|
producer(makeFrame(descriptor, 1, 20));
|
||||||
|
producer(makeFrame(descriptor, 2, 30));
|
||||||
|
|
||||||
|
CHECK_TRUE(subscription.discardPendingIfExceeds(2) == 3);
|
||||||
|
CHECK_TRUE(!subscription.tryRead().has_value());
|
||||||
|
producer(makeFrame(descriptor, 3, 40));
|
||||||
|
const auto next = subscription.tryRead();
|
||||||
|
CHECK_TRUE(next.has_value());
|
||||||
|
CHECK_TRUE(next->value->sequence == 3);
|
||||||
|
CHECK_TRUE(next->dropped_since_last_read == 3);
|
||||||
|
CHECK_TRUE(subscription.droppedCount() == 3);
|
||||||
|
}
|
||||||
|
|
||||||
|
void testHubFailedStartAndShutdown() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
|
||||||
|
std::atomic<int> start_attempts{0};
|
||||||
|
std::atomic<int> retry_stop_count{0};
|
||||||
|
MediaSourceHub::SourceCallbacks failed_callbacks;
|
||||||
|
failed_callbacks.start = [&](const MediaSourceHub::FrameSink&,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
return ++start_attempts >= 2;
|
||||||
|
};
|
||||||
|
failed_callbacks.stop = [&] { ++retry_stop_count; };
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(failed_callbacks), 2));
|
||||||
|
auto failed = hub.subscribe(descriptor->id);
|
||||||
|
CHECK_TRUE(!failed.valid());
|
||||||
|
auto retry = hub.subscribe(descriptor->id);
|
||||||
|
CHECK_TRUE(retry.valid());
|
||||||
|
retry.reset();
|
||||||
|
CHECK_TRUE(start_attempts == 2);
|
||||||
|
CHECK_TRUE(retry_stop_count == 1);
|
||||||
|
CHECK_TRUE(hub.unregisterSource(descriptor->id));
|
||||||
|
|
||||||
|
std::atomic<int> stop_count{0};
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [](const MediaSourceHub::FrameSink&,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
callbacks.stop = [&] { ++stop_count; };
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
|
||||||
|
auto live = hub.subscribe(descriptor->id);
|
||||||
|
CHECK_TRUE(live.valid());
|
||||||
|
hub.shutdown();
|
||||||
|
CHECK_TRUE(stop_count == 1);
|
||||||
|
CHECK_TRUE(!live.valid());
|
||||||
|
CHECK_TRUE(!live.waitRead(50ms).has_value());
|
||||||
|
}
|
||||||
|
|
||||||
|
void testKeyFrameRequestIsOrderedBeforeStop() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::H264, 1);
|
||||||
|
std::atomic<bool> key_frame_entered{false};
|
||||||
|
std::atomic<bool> release_key_frame{false};
|
||||||
|
std::atomic<int> stop_count{0};
|
||||||
|
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [](const MediaSourceHub::FrameSink&,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
callbacks.stop = [&] { ++stop_count; };
|
||||||
|
callbacks.request_key_frame = [&] {
|
||||||
|
key_frame_entered.store(true, std::memory_order_release);
|
||||||
|
while (!release_key_frame.load(std::memory_order_acquire)) {
|
||||||
|
std::this_thread::sleep_for(1ms);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
|
||||||
|
auto subscription = hub.subscribe(descriptor->id);
|
||||||
|
CHECK_TRUE(subscription.valid());
|
||||||
|
|
||||||
|
auto key_frame = std::async(std::launch::async, [&] {
|
||||||
|
return hub.requestKeyFrame(descriptor->id);
|
||||||
|
});
|
||||||
|
const auto enter_deadline = std::chrono::steady_clock::now() + 500ms;
|
||||||
|
while (!key_frame_entered.load(std::memory_order_acquire) &&
|
||||||
|
std::chrono::steady_clock::now() < enter_deadline) {
|
||||||
|
std::this_thread::sleep_for(1ms);
|
||||||
|
}
|
||||||
|
CHECK_TRUE(key_frame_entered.load(std::memory_order_acquire));
|
||||||
|
|
||||||
|
auto stop = std::async(std::launch::async, [&] { subscription.reset(); });
|
||||||
|
CHECK_TRUE(stop.wait_for(20ms) == std::future_status::timeout);
|
||||||
|
CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 0);
|
||||||
|
|
||||||
|
release_key_frame.store(true, std::memory_order_release);
|
||||||
|
CHECK_TRUE(key_frame.get());
|
||||||
|
CHECK_TRUE(stop.wait_for(500ms) == std::future_status::ready);
|
||||||
|
stop.get();
|
||||||
|
CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 1);
|
||||||
|
CHECK_TRUE(!hub.requestKeyFrame(descriptor->id));
|
||||||
|
}
|
||||||
|
|
||||||
|
void testHubCancelsBlockedStartWithoutBlockingShutdown() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
|
||||||
|
std::atomic<bool> start_entered{false};
|
||||||
|
std::atomic<bool> start_exited{false};
|
||||||
|
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [&](const MediaSourceHub::FrameSink&,
|
||||||
|
const MediaSourceHub::CancelPredicate& cancelled) {
|
||||||
|
start_entered.store(true, std::memory_order_release);
|
||||||
|
while (!cancelled()) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
start_exited.store(true, std::memory_order_release);
|
||||||
|
return false;
|
||||||
|
};
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
|
||||||
|
|
||||||
|
auto subscription_future = std::async(std::launch::async, [&] {
|
||||||
|
return hub.subscribe(descriptor->id);
|
||||||
|
});
|
||||||
|
const auto start_deadline = std::chrono::steady_clock::now() + 500ms;
|
||||||
|
while (!start_entered.load(std::memory_order_acquire) &&
|
||||||
|
std::chrono::steady_clock::now() < start_deadline) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
|
||||||
|
auto shutdown_future = std::async(std::launch::async, [&] { hub.shutdown(); });
|
||||||
|
const bool shutdown_completed = shutdown_future.wait_for(500ms) ==
|
||||||
|
std::future_status::ready;
|
||||||
|
if (shutdown_completed) shutdown_future.get();
|
||||||
|
const bool subscribe_completed = subscription_future.wait_for(500ms) ==
|
||||||
|
std::future_status::ready;
|
||||||
|
bool invalid_subscription = false;
|
||||||
|
if (subscribe_completed) {
|
||||||
|
invalid_subscription = !subscription_future.get().valid();
|
||||||
|
}
|
||||||
|
const auto exit_deadline = std::chrono::steady_clock::now() + 500ms;
|
||||||
|
while (!start_exited.load(std::memory_order_acquire) &&
|
||||||
|
std::chrono::steady_clock::now() < exit_deadline) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
|
||||||
|
CHECK_TRUE(start_entered.load(std::memory_order_acquire));
|
||||||
|
CHECK_TRUE(shutdown_completed);
|
||||||
|
CHECK_TRUE(subscribe_completed);
|
||||||
|
CHECK_TRUE(invalid_subscription);
|
||||||
|
CHECK_TRUE(start_exited.load(std::memory_order_acquire));
|
||||||
|
}
|
||||||
|
|
||||||
|
void testHubQuarantinesNonCooperativeStart() {
|
||||||
|
MediaSourceHub hub;
|
||||||
|
const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1);
|
||||||
|
std::atomic<bool> start_entered{false};
|
||||||
|
std::atomic<bool> release_start{false};
|
||||||
|
std::atomic<bool> start_exited{false};
|
||||||
|
std::atomic<int> stop_count{0};
|
||||||
|
|
||||||
|
MediaSourceHub::SourceCallbacks callbacks;
|
||||||
|
callbacks.start = [&](const MediaSourceHub::FrameSink&,
|
||||||
|
const MediaSourceHub::CancelPredicate&) {
|
||||||
|
start_entered.store(true, std::memory_order_release);
|
||||||
|
while (!release_start.load(std::memory_order_acquire)) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
start_exited.store(true, std::memory_order_release);
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
callbacks.stop = [&] { ++stop_count; };
|
||||||
|
CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2));
|
||||||
|
|
||||||
|
auto subscription_future = std::async(std::launch::async, [&] {
|
||||||
|
return hub.subscribe(descriptor->id);
|
||||||
|
});
|
||||||
|
const auto start_deadline = std::chrono::steady_clock::now() + 500ms;
|
||||||
|
while (!start_entered.load(std::memory_order_acquire) &&
|
||||||
|
std::chrono::steady_clock::now() < start_deadline) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto shutdown_started = std::chrono::steady_clock::now();
|
||||||
|
hub.shutdown();
|
||||||
|
const bool shutdown_was_bounded =
|
||||||
|
std::chrono::steady_clock::now() - shutdown_started < 500ms;
|
||||||
|
const bool subscribe_completed = subscription_future.wait_for(500ms) ==
|
||||||
|
std::future_status::ready;
|
||||||
|
bool invalid_subscription = false;
|
||||||
|
if (subscribe_completed) {
|
||||||
|
invalid_subscription = !subscription_future.get().valid();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Release the deliberately non-cooperative test callback before its stack
|
||||||
|
// captures go out of scope. Late successful startup must be stopped once.
|
||||||
|
release_start.store(true, std::memory_order_release);
|
||||||
|
const auto exit_deadline = std::chrono::steady_clock::now() + 500ms;
|
||||||
|
while ((!start_exited.load(std::memory_order_acquire) || stop_count.load() != 1) &&
|
||||||
|
std::chrono::steady_clock::now() < exit_deadline) {
|
||||||
|
std::this_thread::sleep_for(2ms);
|
||||||
|
}
|
||||||
|
|
||||||
|
CHECK_TRUE(start_entered.load(std::memory_order_acquire));
|
||||||
|
CHECK_TRUE(shutdown_was_bounded);
|
||||||
|
CHECK_TRUE(subscribe_completed);
|
||||||
|
CHECK_TRUE(invalid_subscription);
|
||||||
|
CHECK_TRUE(start_exited.load(std::memory_order_acquire));
|
||||||
|
CHECK_TRUE(stop_count.load() == 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int main() {
|
||||||
|
testMediaMetadataValidation();
|
||||||
|
testLegacySpmcCompatibility();
|
||||||
|
testBroadcastFrameRing();
|
||||||
|
testBroadcastDiscardPending();
|
||||||
|
testBroadcastDiscardConcurrentPublish();
|
||||||
|
testBroadcastConcurrency();
|
||||||
|
testHubLifecycleAndDescriptorRefresh();
|
||||||
|
testSubscriptionDiscardPending();
|
||||||
|
testHubFailedStartAndShutdown();
|
||||||
|
testKeyFrameRequestIsOrderedBeforeStop();
|
||||||
|
testHubCancelsBlockedStartWithoutBlockingShutdown();
|
||||||
|
testHubQuarantinesNonCooperativeStart();
|
||||||
|
|
||||||
|
if (failures != 0) {
|
||||||
|
std::cerr << failures << " media_source_hub checks failed\n";
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
std::cout << "media_source_hub self-test passed\n";
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@ -38,6 +38,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
|
|||||||
return "TASK_TYPE_GRPC_SERVER";
|
return "TASK_TYPE_GRPC_SERVER";
|
||||||
case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION:
|
case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION:
|
||||||
return "TASK_TYPE_SELF_COLLISION";
|
return "TASK_TYPE_SELF_COLLISION";
|
||||||
|
case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE:
|
||||||
|
return "TASK_TYPE_QUIC_EDGE";
|
||||||
case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
|
case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
|
||||||
default:
|
default:
|
||||||
return "TASK_TYPE_UNKNOWN";
|
return "TASK_TYPE_UNKNOWN";
|
||||||
|
|||||||
@ -12,6 +12,7 @@ target_link_libraries(cmvr_runtime PUBLIC
|
|||||||
cmvr_es::device_manager
|
cmvr_es::device_manager
|
||||||
cmvr_es::task_manager
|
cmvr_es::task_manager
|
||||||
cmvr_es::service
|
cmvr_es::service
|
||||||
|
cmvr_es::quic_edge_task
|
||||||
cmvr_es::mujoco_viewer
|
cmvr_es::mujoco_viewer
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -11,6 +11,7 @@
|
|||||||
#include "common/config/config_files.h"
|
#include "common/config/config_files.h"
|
||||||
#include "common/io/proto_file_io.h"
|
#include "common/io/proto_file_io.h"
|
||||||
#include "task/grpc_server_task/include/grpc_server_task.h"
|
#include "task/grpc_server_task/include/grpc_server_task.h"
|
||||||
|
#include "task/quic_edge_task/include/quic_edge_task.h"
|
||||||
|
|
||||||
namespace cmvr {
|
namespace cmvr {
|
||||||
namespace {
|
namespace {
|
||||||
@ -102,6 +103,7 @@ bool Runtime::init_(const std::string& config_path,
|
|||||||
device::DeviceManager::getInstance(device_manager_root.device_manager());
|
device::DeviceManager::getInstance(device_manager_root.device_manager());
|
||||||
|
|
||||||
task::registerGrpcServerTaskFactory();
|
task::registerGrpcServerTaskFactory();
|
||||||
|
task::registerQuicEdgeTaskFactory();
|
||||||
|
|
||||||
if (app_config.task_manager_config_file().empty()) {
|
if (app_config.task_manager_config_file().empty()) {
|
||||||
CMVR_LOG(ERROR) << "TaskManager config file is empty";
|
CMVR_LOG(ERROR) << "TaskManager config file is empty";
|
||||||
|
|||||||
@ -7,6 +7,7 @@ add_library(service
|
|||||||
grpc/src/grpc_head_service.cpp
|
grpc/src/grpc_head_service.cpp
|
||||||
grpc/src/grpc_dexhand_service.cpp
|
grpc/src/grpc_dexhand_service.cpp
|
||||||
grpc/src/grpc_arm_service.cpp
|
grpc/src/grpc_arm_service.cpp
|
||||||
|
grpc/src/grpc_agv_service.cpp
|
||||||
grpc/src/grpc_hlc_service.cpp
|
grpc/src/grpc_hlc_service.cpp
|
||||||
../task/grpc_server_task/src/grpc_server_task.cpp
|
../task/grpc_server_task/src/grpc_server_task.cpp
|
||||||
)
|
)
|
||||||
@ -20,12 +21,29 @@ target_link_libraries(service PRIVATE
|
|||||||
cmvr_es::task_manager
|
cmvr_es::task_manager
|
||||||
cmvr_es::algorithms::controller
|
cmvr_es::algorithms::controller
|
||||||
cmvr_es::task
|
cmvr_es::task
|
||||||
|
cmvr_es::media_source_hub
|
||||||
|
cmvr_es::device_media_source_adapter
|
||||||
protobuf::libprotobuf
|
protobuf::libprotobuf
|
||||||
)
|
)
|
||||||
|
|
||||||
add_library(cmvr_es::service ALIAS service)
|
add_library(cmvr_es::service ALIAS service)
|
||||||
install(TARGETS service LIBRARY DESTINATION lib)
|
install(TARGETS service LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
add_executable(grpc_camera_stream_policy_test
|
||||||
|
grpc/tests/grpc_camera_stream_policy_test.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(grpc_camera_stream_policy_test
|
||||||
|
PRIVATE
|
||||||
|
${CMAKE_SOURCE_DIR}/cmvr-es
|
||||||
|
)
|
||||||
|
add_test(
|
||||||
|
NAME grpc_camera_stream_policy_test
|
||||||
|
COMMAND grpc_camera_stream_policy_test
|
||||||
|
)
|
||||||
|
set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10)
|
||||||
|
endif()
|
||||||
|
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
# Unit test
|
# Unit test
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
233
cmvr-es/service/README.md
Normal file
233
cmvr-es/service/README.md
Normal file
@ -0,0 +1,233 @@
|
|||||||
|
# Service 模块开发指南
|
||||||
|
|
||||||
|
`service/` 实现边缘端对外协议和设备抽象之间的适配。Service 负责解析请求、查找设备、转换 DTO 和返回结果，不负责创建具体设备后端。
|
||||||
|
|
||||||
|
返回[项目总览](../../README.md)。
|
||||||
|
|
||||||
|
## 当前结构
|
||||||
|
|
||||||
|
| 目录 | 职责 |
|
||||||
|
| --- | --- |
|
||||||
|
| `grpc/` | 入站设备控制、状态查询和兼容流式接口 |
|
||||||
|
| `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer |
|
||||||
|
| `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 |
|
||||||
|
|
||||||
|
两个遗留 gRPC client test 位于 `grpc/src/*_client_test.cpp`，当前没有通过 `add_test()` 登记。
|
||||||
|
|
||||||
|
gRPC 和 QUIC 的职责边界：
|
||||||
|
|
||||||
|
- 机械臂、AGV 等可靠控制继续使用 gRPC；
|
||||||
|
- 节点注册、心跳和 IP 上报使用 QUIC reliable stream；
|
||||||
|
- 实时音视频使用 QUIC DATAGRAM；
|
||||||
|
- `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。
|
||||||
|
|
||||||
|
## 新增 gRPC Service
|
||||||
|
|
||||||
|
当前没有动态 service registry，必须完成以下全部步骤。
|
||||||
|
|
||||||
|
### 1. 定义 Proto
|
||||||
|
|
||||||
|
在 [`../../protos/cmvr/api/`](../../protos/cmvr/api/) 增加或扩展：
|
||||||
|
|
||||||
|
- `<domain>_command.proto`
|
||||||
|
- `<domain>_service.proto`
|
||||||
|
|
||||||
|
import 路径必须相对于 `protos/`。兼容规则见 [`../../protos/README.md`](../../protos/README.md)。
|
||||||
|
|
||||||
|
### 2. 实现 Service
|
||||||
|
|
||||||
|
目录约定：
|
||||||
|
|
||||||
|
```text
|
||||||
|
service/grpc/
|
||||||
|
├── include/grpc_example_service.h
|
||||||
|
└── src/grpc_example_service.cpp
|
||||||
|
```
|
||||||
|
|
||||||
|
实现类继承生成的：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
cmvr::api::ExampleService::Service
|
||||||
|
```
|
||||||
|
|
||||||
|
通过已初始化的 `DeviceManager` 获取抽象设备。不要在 service 中创建厂商 SDK 对象，不要绕过设备 factory。
|
||||||
|
|
||||||
|
### 3. 加入 service target
|
||||||
|
|
||||||
|
将实现 `.cpp` 加入 [`CMakeLists.txt`](CMakeLists.txt) 的 `service` library，并声明最小依赖。
|
||||||
|
|
||||||
|
### 4. 注册到 GrpcServerTask
|
||||||
|
|
||||||
|
还必须修改：
|
||||||
|
|
||||||
|
- [`../task/grpc_server_task/include/grpc_server_task.h`](../task/grpc_server_task/include/grpc_server_task.h)
|
||||||
|
- [`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp)
|
||||||
|
|
||||||
|
完成：
|
||||||
|
|
||||||
|
1. 增加 service owner；
|
||||||
|
2. 在 start 中构造；
|
||||||
|
3. 调用 `builder.RegisterService(...)`；
|
||||||
|
4. 在 `clearServices()` 中 reset。
|
||||||
|
|
||||||
|
漏掉该步骤时项目可能编译成功，但服务不会出现在 reflection 或运行时。
|
||||||
|
|
||||||
|
### 5. 测试
|
||||||
|
|
||||||
|
- 直接测试 service handler 或启动临时 gRPC server；
|
||||||
|
- 覆盖设备不存在、类型不匹配、设备错误和取消；
|
||||||
|
- 使用 grpcurl/reflection 验证服务全名；
|
||||||
|
- 流式 RPC 覆盖客户端断开和慢消费者；
|
||||||
|
- 在 CMake 中使用 `if(BUILD_TESTING)` 包裹测试目标，并通过 `add_test()` 登记。
|
||||||
|
|
||||||
|
`grpc_arm_client_test` 和 `grpc_hlc_client_test` 是未登记到 CTest 的历史可执行文件，不能代表默认自动覆盖。
|
||||||
|
|
||||||
|
## gRPC 实现约束
|
||||||
|
|
||||||
|
### 错误语义
|
||||||
|
|
||||||
|
当前历史服务存在两种风格：
|
||||||
|
|
||||||
|
- gRPC status 返回 OK，业务失败写入 Feedback header；
|
||||||
|
- 使用非 OK gRPC status 表达 transport/API 失败。
|
||||||
|
|
||||||
|
扩展已有服务时保持其兼容语义。新增服务必须在设计时明确：
|
||||||
|
|
||||||
|
- 哪些错误使用 gRPC status；
|
||||||
|
- 哪些错误使用业务 Feedback；
|
||||||
|
- 是否允许部分成功；
|
||||||
|
- deadline/cancellation 如何映射；
|
||||||
|
- 不得同时返回互相矛盾的 transport 和业务状态。
|
||||||
|
|
||||||
|
### 流式 RPC
|
||||||
|
|
||||||
|
- 检查 `context->IsCancelled()`；
|
||||||
|
- 检查 `Read()` / `Write()` 返回；
|
||||||
|
- 使用 RAII 或 MediaSourceHub Subscription 释放 producer lease；
|
||||||
|
- 不持有设备状态锁进行网络写；
|
||||||
|
- 为 wait/read 使用有限 timeout；
|
||||||
|
- 慢客户端不能阻塞设备生产线程；
|
||||||
|
- H.264/H.265 丢帧后等待关键帧恢复；
|
||||||
|
- gRPC RGB 流在积压超过 `camera_stream_max_pending_frames` 或帧龄超过
|
||||||
|
`camera_stream_max_frame_age_ms` 时主动丢弃旧帧，请求 IDR，并从下一个关键帧恢复。
|
||||||
|
|
||||||
|
当前仅 gRPC RGB 和麦克风流使用 MediaSourceHub；Depth/RGBD 仍直接读取设备帧。
|
||||||
|
|
||||||
|
gRPC 相机实时流默认最多保留 2 帧积压、最大允许 250 ms 帧龄。两个配置项填 0
|
||||||
|
时使用上述默认值。该策略以低延迟为目标，不保证每个视频帧都到达客户端；控制命令
|
||||||
|
仍由普通 gRPC RPC 承担。
|
||||||
|
|
||||||
|
`FrameData` 附带 `capture_utc_ns`、`source_sequence`、`pts`、`dts`、
|
||||||
|
`source_fps`、`source_timestamp` 和 `source_frame_number`。平台端可用采集时间
|
||||||
|
与接收时间的差值区分设备、网络、服务端写阻塞和客户端解码/渲染队列延迟。新增字段
|
||||||
|
保持 protobuf wire compatibility，旧客户端可以继续连接，但需要重新生成代码后才能
|
||||||
|
读取这些诊断字段。
|
||||||
|
|
||||||
|
### 当前安全状态
|
||||||
|
|
||||||
|
GrpcServerTask 使用同步 `grpc::ServerBuilder` 和 `grpc::InsecureServerCredentials()`。reflection 由配置控制。当前没有 gRPC TLS、认证、授权或标准 health service。
|
||||||
|
|
||||||
|
QUIC 配置中的 `grpc_endpoint_tls` 只是上报字段，不会启用 gRPC TLS。
|
||||||
|
|
||||||
|
## 扩展 QUIC Edge
|
||||||
|
|
||||||
|
关键层次：
|
||||||
|
|
||||||
|
| 层 | 主要文件 |
|
||||||
|
| --- | --- |
|
||||||
|
| 控制流 framing | `quic_edge/src/control_framing.cpp` |
|
||||||
|
| DATAGRAM 固定头和分片 | `quic_edge/src/datagram_packetizer.cpp` |
|
||||||
|
| 会话和状态机 | `quic_edge/src/quic_edge_service.cpp` |
|
||||||
|
| 传输抽象 | `quic_edge/include/quic_transport.h` |
|
||||||
|
| MsQuic 后端 | `quic_edge/src/msquic_transport.cpp` |
|
||||||
|
| 设备媒体适配 | `quic_edge/src/quic_edge_device_adapter.cpp` |
|
||||||
|
| DeviceManager 心跳适配 | `quic_edge/src/quic_edge_device_adapter.cpp` |
|
||||||
|
|
||||||
|
线协议见 [`../../protos/cmvr/quic_edge/v1/README.md`](../../protos/cmvr/quic_edge/v1/README.md)。
|
||||||
|
|
||||||
|
### 新增 Transport 后端
|
||||||
|
|
||||||
|
1. 实现 `QuicTransport` 完整接口；
|
||||||
|
2. 明确 callback 所在线程；
|
||||||
|
3. stop/close 后不得再访问已销毁 service；
|
||||||
|
4. `QUEUED` 表示 transport 接管待发送数据；
|
||||||
|
5. `WOULD_BLOCK` 或 `ERROR` 不得接管任何字节；
|
||||||
|
6. DATAGRAM batch 本地准入必须原子；
|
||||||
|
7. native send 部分失败时关闭连接并清理 session；
|
||||||
|
8. 添加 fake transport 故障注入测试；
|
||||||
|
9. 在默认 transport factory 中显式选择后端。
|
||||||
|
|
||||||
|
### 新增控制消息
|
||||||
|
|
||||||
|
不能只修改 Proto，还要同步：
|
||||||
|
|
||||||
|
- envelope 构造与发送；
|
||||||
|
- 入站 dispatch；
|
||||||
|
- 合法状态和消息时序；
|
||||||
|
- message sequence 校验；
|
||||||
|
- session ID 和 heartbeat sequence 校验；
|
||||||
|
- reconnect 后状态清理；
|
||||||
|
- Java Gateway 对端；
|
||||||
|
- framing、状态机和 fake transport 测试。
|
||||||
|
|
||||||
|
当前 Edge 入站只接受：
|
||||||
|
|
||||||
|
- `NodeRegisterResponse`
|
||||||
|
- `NodeHeartbeatAck`
|
||||||
|
- `ProtocolError`
|
||||||
|
|
||||||
|
虽然 Proto 定义了 `MediaSessionClose`，本版本 Edge 收到它仍会判为 unexpected，不应将其描述为已实现的双向控制能力。
|
||||||
|
|
||||||
|
### DeviceManager 心跳快照
|
||||||
|
|
||||||
|
`QuicEdgeService` 通过可注入的 `DeviceSnapshotProvider` 获取协议无关的纯值
|
||||||
|
快照。生产构造绑定已经初始化的 `DeviceManager`，fake transport 测试则注入
|
||||||
|
合成快照，因此协议状态机不需要创建硬件对象或依赖 DeviceManager 单例。
|
||||||
|
DeviceManager 的本地快照继续保留禁用设备；QUIC wire 映射层仅序列化
|
||||||
|
`enabled=true` 的设备。已启用但创建、初始化或启动失败的设备不会被过滤。
|
||||||
|
|
||||||
|
心跳线程只读取 Manager 维护的内存状态，不能在这里同步访问厂商 SDK、网络或
|
||||||
|
设备总线。新增设备健康探针必须实现 `AbstractDevice::healthSnapshot()` 的
|
||||||
|
线程安全、无阻塞 I/O 契约；未实现时上报 `UNSPECIFIED`，不得伪造为健康。
|
||||||
|
设备异常字符串会限长，整条消息仍受 `maximum_control_frame_bytes` 约束。
|
||||||
|
|
||||||
|
### 跨 QUIC 通道顺序
|
||||||
|
|
||||||
|
Edge 会先调用可靠流发送 session/descriptor，再调用 DATAGRAM 发送媒体，但 QUIC stream 与 DATAGRAM 没有跨通道到达顺序保证。
|
||||||
|
|
||||||
|
Gateway 必须容忍 DATAGRAM 先到，对未知 session epoch 或 codec generation 的数据有界暂存或丢弃。
|
||||||
|
|
||||||
|
## QUIC 测试要求
|
||||||
|
|
||||||
|
参考 [`quic_edge/CMakeLists.txt`](quic_edge/CMakeLists.txt) 和 `quic_edge_protocol_test`，至少覆盖：
|
||||||
|
|
||||||
|
- 控制消息拆包、粘包和超限；
|
||||||
|
- 重复或倒退 sequence；
|
||||||
|
- 注册、ACK 超时和重连；
|
||||||
|
- DeviceManager 已启用设备过滤、类型/状态映射和 provider 失败隔离；
|
||||||
|
- Gateway 返回零心跳周期时采用本地 `heartbeat_interval_ms`；
|
||||||
|
- session epoch 清理；
|
||||||
|
- DATAGRAM header 字节序；
|
||||||
|
- 分片边界和超大帧；
|
||||||
|
- 原子队列准入和背压；
|
||||||
|
- 丢帧、generation 和关键帧恢复；
|
||||||
|
- stop 与 callback 并发。
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cmake --build build --target quic_edge_protocol_test
|
||||||
|
ctest \
|
||||||
|
--test-dir build \
|
||||||
|
-R '^quic_edge_protocol_test$' \
|
||||||
|
--output-on-failure
|
||||||
|
```
|
||||||
|
|
||||||
|
## 提交检查
|
||||||
|
|
||||||
|
- [ ] service 不创建具体硬件后端
|
||||||
|
- [ ] Proto、实现、CMake 和 GrpcServerTask 注册均已更新
|
||||||
|
- [ ] 错误语义与已有服务兼容
|
||||||
|
- [ ] deadline、cancel、Read/Write 失败均处理
|
||||||
|
- [ ] 流式资源通过 RAII 释放
|
||||||
|
- [ ] gRPC 安全能力没有被配置字段误描述
|
||||||
|
- [ ] QUIC 状态机与 Gateway 同步更新
|
||||||
|
- [ ] 协议测试已登记到 CTest
|
||||||
82
cmvr-es/service/grpc/include/grpc_agv_service.h
Normal file
82
cmvr-es/service/grpc/include/grpc_agv_service.h
Normal file
@ -0,0 +1,82 @@
|
|||||||
|
#ifndef CMVR_ES_GRPC_AGV_SERVICE_H
|
||||||
|
#define CMVR_ES_GRPC_AGV_SERVICE_H
|
||||||
|
|
||||||
|
#include "cmvr/api/agv_service.grpc.pb.h"
|
||||||
|
#include "devices/agv/abstract_agv.h"
|
||||||
|
#include "manager/device_manager/include/device_manager.h"
|
||||||
|
|
||||||
|
namespace cmvr::service {
|
||||||
|
|
||||||
|
class gRPCAgvServiceImpl final : public api::AgvService::Service {
|
||||||
|
public:
|
||||||
|
gRPCAgvServiceImpl();
|
||||||
|
~gRPCAgvServiceImpl() override = default;
|
||||||
|
|
||||||
|
grpc::Status getRuntimeState(grpc::ServerContext* context,
|
||||||
|
const api::AgvRuntimeStateCommand_Request* request,
|
||||||
|
api::AgvRuntimeStateCommand_Feedback* response) override;
|
||||||
|
grpc::Status getNavigationStatus(grpc::ServerContext* context,
|
||||||
|
const api::AgvNavigationStatusCommand_Request* request,
|
||||||
|
api::AgvNavigationStatusCommand_Feedback* response) override;
|
||||||
|
grpc::Status emergencyStop(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status clearFault(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status navigateToPose(grpc::ServerContext* context,
|
||||||
|
const api::AgvNavigateToPoseCommand_Request* request,
|
||||||
|
api::AgvNavigateToPoseCommand_Feedback* response) override;
|
||||||
|
grpc::Status navigateToStation(grpc::ServerContext* context,
|
||||||
|
const api::AgvNavigateToStationCommand_Request* request,
|
||||||
|
api::AgvNavigateToStationCommand_Feedback* response) override;
|
||||||
|
grpc::Status followPath(grpc::ServerContext* context,
|
||||||
|
const api::AgvFollowPathCommand_Request* request,
|
||||||
|
api::AgvFollowPathCommand_Feedback* response) override;
|
||||||
|
grpc::Status pauseNavigation(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status resumeNavigation(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status cancelNavigation(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status setVelocity(grpc::ServerContext* context,
|
||||||
|
const api::AgvSetVelocityCommand_Request* request,
|
||||||
|
api::AgvSetVelocityCommand_Feedback* response) override;
|
||||||
|
grpc::Status stopVelocityControl(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
grpc::Status listMaps(grpc::ServerContext* context,
|
||||||
|
const api::AgvListMapsCommand_Request* request,
|
||||||
|
api::AgvListMapsCommand_Feedback* response) override;
|
||||||
|
grpc::Status listStations(grpc::ServerContext* context,
|
||||||
|
const api::AgvListStationsCommand_Request* request,
|
||||||
|
api::AgvListStationsCommand_Feedback* response) override;
|
||||||
|
grpc::Status switchMap(grpc::ServerContext* context,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response) override;
|
||||||
|
grpc::Status uploadMap(grpc::ServerContext* context,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response) override;
|
||||||
|
grpc::Status downloadMap(grpc::ServerContext* context,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response) override;
|
||||||
|
grpc::Status startMapping(grpc::ServerContext* context,
|
||||||
|
const api::AgvStartMappingCommand_Request* request,
|
||||||
|
api::AgvStartMappingCommand_Feedback* response) override;
|
||||||
|
grpc::Status streamMap(grpc::ServerContext* context,
|
||||||
|
const api::AgvMapStreamCommand_Request* request,
|
||||||
|
grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer) override;
|
||||||
|
grpc::Status stopMapping(grpc::ServerContext* context,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
device::DeviceManager& dmgr_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::service
|
||||||
|
|
||||||
|
#endif // CMVR_ES_GRPC_AGV_SERVICE_H
|
||||||
@ -50,6 +50,9 @@ public:
|
|||||||
grpc::Status computeForwardKinematics(grpc::ServerContext* context,
|
grpc::Status computeForwardKinematics(grpc::ServerContext* context,
|
||||||
const api::ComputeForwardKinematics_Request* request,
|
const api::ComputeForwardKinematics_Request* request,
|
||||||
api::ComputeForwardKinematics_Response* response) override;
|
api::ComputeForwardKinematics_Response* response) override;
|
||||||
|
grpc::Status clearFault(grpc::ServerContext *context,
|
||||||
|
const cmvr::api::CommandHeader_Request *request,
|
||||||
|
cmvr::api::CommandHeader_Feedback *response) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
device::DeviceManager& dmgr_;
|
device::DeviceManager& dmgr_;
|
||||||
|
|||||||
@ -9,12 +9,14 @@
|
|||||||
#include "common/base/grpc_utils.h"
|
#include "common/base/grpc_utils.h"
|
||||||
#include "manager/device_manager/include/device_manager.h"
|
#include "manager/device_manager/include/device_manager.h"
|
||||||
#include "devices/camera/abstract_camera.h"
|
#include "devices/camera/abstract_camera.h"
|
||||||
|
#include "service/grpc/include/grpc_camera_stream_policy.h"
|
||||||
|
|
||||||
namespace cmvr::service {
|
namespace cmvr::service {
|
||||||
|
|
||||||
class gRPCCameraServiceImpl final: public api::CameraService::Service {
|
class gRPCCameraServiceImpl final: public api::CameraService::Service {
|
||||||
public:
|
public:
|
||||||
gRPCCameraServiceImpl();
|
explicit gRPCCameraServiceImpl(
|
||||||
|
CameraStreamLowLatencyConfig stream_config = {});
|
||||||
~gRPCCameraServiceImpl() override = default;
|
~gRPCCameraServiceImpl() override = default;
|
||||||
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
|
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
|
||||||
grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
|
grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
|
||||||
@ -24,11 +26,13 @@ namespace cmvr::service {
|
|||||||
grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
|
grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
|
||||||
grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
|
grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
|
||||||
grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
|
grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
|
||||||
|
grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override;
|
||||||
grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
|
grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
|
||||||
grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
|
grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
|
||||||
grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
|
grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
|
||||||
private:
|
private:
|
||||||
device::DeviceManager& dmgr_;
|
device::DeviceManager& dmgr_;
|
||||||
|
CameraStreamLowLatencyConfig stream_config_;
|
||||||
|
|
||||||
//双向流读写线程
|
//双向流读写线程
|
||||||
std::shared_ptr<std::thread> read_thread_ = nullptr;
|
std::shared_ptr<std::thread> read_thread_ = nullptr;
|
||||||
|
|||||||
60
cmvr-es/service/grpc/include/grpc_camera_stream_policy.h
Normal file
60
cmvr-es/service/grpc/include/grpc_camera_stream_policy.h
Normal file
@ -0,0 +1,60 @@
|
|||||||
|
#ifndef CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||||
|
#define CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <optional>
|
||||||
|
|
||||||
|
namespace cmvr::service {
|
||||||
|
|
||||||
|
inline constexpr size_t kDefaultCameraStreamMaxPendingFrames = 2;
|
||||||
|
inline constexpr uint32_t kDefaultCameraStreamMaxFrameAgeMs = 250;
|
||||||
|
|
||||||
|
struct CameraStreamLowLatencyConfig {
|
||||||
|
size_t max_pending_frames{kDefaultCameraStreamMaxPendingFrames};
|
||||||
|
std::chrono::milliseconds max_frame_age{
|
||||||
|
kDefaultCameraStreamMaxFrameAgeMs};
|
||||||
|
};
|
||||||
|
|
||||||
|
inline CameraStreamLowLatencyConfig makeCameraStreamLowLatencyConfig(
|
||||||
|
const uint32_t max_pending_frames,
|
||||||
|
const uint32_t max_frame_age_ms) noexcept {
|
||||||
|
CameraStreamLowLatencyConfig config;
|
||||||
|
config.max_pending_frames = max_pending_frames == 0
|
||||||
|
? kDefaultCameraStreamMaxPendingFrames
|
||||||
|
: static_cast<size_t>(max_pending_frames);
|
||||||
|
config.max_frame_age = std::chrono::milliseconds(
|
||||||
|
max_frame_age_ms == 0
|
||||||
|
? kDefaultCameraStreamMaxFrameAgeMs
|
||||||
|
: max_frame_age_ms);
|
||||||
|
return config;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::optional<uint64_t> cameraFrameAgeNs(
|
||||||
|
const uint64_t capture_time_ns,
|
||||||
|
const uint64_t now_ns) noexcept {
|
||||||
|
if (capture_time_ns == 0 || now_ns < capture_time_ns) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return now_ns - capture_time_ns;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool cameraFrameExceedsAgeLimit(
|
||||||
|
const uint64_t capture_time_ns,
|
||||||
|
const uint64_t now_ns,
|
||||||
|
const std::chrono::milliseconds max_frame_age) noexcept {
|
||||||
|
const auto age_ns = cameraFrameAgeNs(capture_time_ns, now_ns);
|
||||||
|
if (!age_ns || max_frame_age.count() <= 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const auto max_age_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
max_frame_age).count();
|
||||||
|
return *age_ns > static_cast<uint64_t>(max_age_ns);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::service
|
||||||
|
|
||||||
|
#endif // CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H
|
||||||
@ -22,6 +22,7 @@ namespace cmvr::service
|
|||||||
grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override;
|
grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override;
|
||||||
grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override;
|
grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override;
|
||||||
grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override;
|
grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override;
|
||||||
|
grpc::Status StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) override;
|
||||||
grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override;
|
grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override;
|
||||||
grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override;
|
grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override;
|
||||||
private:
|
private:
|
||||||
|
|||||||
@ -17,6 +17,7 @@ namespace cmvr::service {
|
|||||||
~gRPCSpeakerServiceImpl() override = default;
|
~gRPCSpeakerServiceImpl() override = default;
|
||||||
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override;
|
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override;
|
||||||
grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override;
|
grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override;
|
||||||
|
grpc::Status StreamAudio(grpc::ServerContext* context, grpc::ServerReader<api::StreamSpeakerAudioCommand_Request>* reader, api::StreamSpeakerAudioCommand_Feedback* response) override;
|
||||||
grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override;
|
grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override;
|
||||||
grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override;
|
grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override;
|
||||||
grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override;
|
grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override;
|
||||||
|
|||||||
@ -18,6 +18,7 @@ namespace cmvr::service
|
|||||||
grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override;
|
grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override;
|
||||||
grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override;
|
grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override;
|
||||||
grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
|
grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
|
||||||
|
grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) override;
|
||||||
grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override;
|
grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override;
|
||||||
private:
|
private:
|
||||||
device::DeviceManager& dmgr_;
|
device::DeviceManager& dmgr_;
|
||||||
|
|||||||
752
cmvr-es/service/grpc/src/grpc_agv_service.cpp
Normal file
752
cmvr-es/service/grpc/src/grpc_agv_service.cpp
Normal file
@ -0,0 +1,752 @@
|
|||||||
|
#include "service/grpc/include/grpc_agv_service.h"
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <exception>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <google/protobuf/util/time_util.h>
|
||||||
|
|
||||||
|
using google::protobuf::util::TimeUtil;
|
||||||
|
|
||||||
|
namespace cmvr::service {
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
void fillFeedback(api::CommandHeader_Feedback* feedback,
|
||||||
|
const bool success,
|
||||||
|
const std::string& message = {})
|
||||||
|
{
|
||||||
|
feedback->set_success(success);
|
||||||
|
feedback->set_error_message(message);
|
||||||
|
*feedback->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status resultToStatus(const device::AgvResult& result)
|
||||||
|
{
|
||||||
|
if (result.ok()) {
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename Response>
|
||||||
|
grpc::Status setResponseResult(Response* response, const device::AgvResult& result)
|
||||||
|
{
|
||||||
|
fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message);
|
||||||
|
return resultToStatus(result);
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status setResponseResult(api::CommandHeader_Feedback* response, const device::AgvResult& result)
|
||||||
|
{
|
||||||
|
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||||
|
return resultToStatus(result);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename Response>
|
||||||
|
grpc::Status setDeviceNotFound(Response* response, const std::string& device_id)
|
||||||
|
{
|
||||||
|
const std::string message = "AGV device not found: " + device_id;
|
||||||
|
fillFeedback(response->mutable_header(), false, message);
|
||||||
|
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id)
|
||||||
|
{
|
||||||
|
const std::string message = "AGV device not found: " + device_id;
|
||||||
|
fillFeedback(response, false, message);
|
||||||
|
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src)
|
||||||
|
{
|
||||||
|
device::AgvAdapterParams dst;
|
||||||
|
for (const auto& [key, value] : src.values()) {
|
||||||
|
dst.values.emplace(key, value);
|
||||||
|
}
|
||||||
|
return dst;
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src)
|
||||||
|
{
|
||||||
|
device::AgvMotionOptions dst;
|
||||||
|
dst.max_speed = src.max_speed();
|
||||||
|
dst.max_angular_speed = src.max_angular_speed();
|
||||||
|
dst.max_acceleration = src.max_acceleration();
|
||||||
|
dst.max_angular_acceleration = src.max_angular_acceleration();
|
||||||
|
dst.reach_distance = src.reach_distance();
|
||||||
|
dst.reach_angle = src.reach_angle();
|
||||||
|
dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0;
|
||||||
|
dst.asynchronous = src.asynchronous();
|
||||||
|
return dst;
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvVelocity toVelocity(const msgs::AgvVelocity& src)
|
||||||
|
{
|
||||||
|
return {src.vx(), src.vy(), src.wz()};
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src)
|
||||||
|
{
|
||||||
|
device::AgvPathSegment dst;
|
||||||
|
dst.source_station = src.source_station();
|
||||||
|
dst.target_station = src.target_station();
|
||||||
|
return dst;
|
||||||
|
}
|
||||||
|
|
||||||
|
math::Pose2d toPose2d(const msgs::AgvPose2d& src)
|
||||||
|
{
|
||||||
|
return {src.x(), src.y(), src.theta()};
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvMapDimension toMapDimension(const msgs::AgvMapDimension src)
|
||||||
|
{
|
||||||
|
switch (src) {
|
||||||
|
case msgs::AGV_MAP_2D:
|
||||||
|
return device::AgvMapDimension::Map2D;
|
||||||
|
case msgs::AGV_MAP_3D:
|
||||||
|
return device::AgvMapDimension::Map3D;
|
||||||
|
case msgs::AGV_MAP_2D_AND_3D:
|
||||||
|
return device::AgvMapDimension::Map2DAnd3D;
|
||||||
|
case msgs::AGV_MAP_DIMENSION_UNSPECIFIED:
|
||||||
|
default:
|
||||||
|
return device::AgvMapDimension::Unspecified;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msgs::AgvMapDimension toProtoMapDimension(const device::AgvMapDimension src)
|
||||||
|
{
|
||||||
|
switch (src) {
|
||||||
|
case device::AgvMapDimension::Map2D:
|
||||||
|
return msgs::AGV_MAP_2D;
|
||||||
|
case device::AgvMapDimension::Map3D:
|
||||||
|
return msgs::AGV_MAP_3D;
|
||||||
|
case device::AgvMapDimension::Map2DAnd3D:
|
||||||
|
return msgs::AGV_MAP_2D_AND_3D;
|
||||||
|
case device::AgvMapDimension::Unspecified:
|
||||||
|
default:
|
||||||
|
return msgs::AGV_MAP_DIMENSION_UNSPECIFIED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msgs::AgvMapUpdateType toProtoMapUpdateType(const device::AgvMapUpdateType src)
|
||||||
|
{
|
||||||
|
switch (src) {
|
||||||
|
case device::AgvMapUpdateType::Snapshot:
|
||||||
|
return msgs::AGV_MAP_UPDATE_SNAPSHOT;
|
||||||
|
case device::AgvMapUpdateType::Incremental:
|
||||||
|
return msgs::AGV_MAP_UPDATE_INCREMENTAL;
|
||||||
|
case device::AgvMapUpdateType::Reset:
|
||||||
|
return msgs::AGV_MAP_UPDATE_RESET;
|
||||||
|
case device::AgvMapUpdateType::Unspecified:
|
||||||
|
default:
|
||||||
|
return msgs::AGV_MAP_UPDATE_UNSPECIFIED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msgs::AgvMapObjectType toProtoMapObjectType(const device::AgvMapObjectType src)
|
||||||
|
{
|
||||||
|
switch (src) {
|
||||||
|
case device::AgvMapObjectType::Station:
|
||||||
|
return msgs::AGV_MAP_OBJECT_STATION;
|
||||||
|
case device::AgvMapObjectType::Line:
|
||||||
|
return msgs::AGV_MAP_OBJECT_LINE;
|
||||||
|
case device::AgvMapObjectType::Area:
|
||||||
|
return msgs::AGV_MAP_OBJECT_AREA;
|
||||||
|
case device::AgvMapObjectType::QrTag:
|
||||||
|
return msgs::AGV_MAP_OBJECT_QR_TAG;
|
||||||
|
case device::AgvMapObjectType::Reflector:
|
||||||
|
return msgs::AGV_MAP_OBJECT_REFLECTOR;
|
||||||
|
case device::AgvMapObjectType::BinLocation:
|
||||||
|
return msgs::AGV_MAP_OBJECT_BIN_LOCATION;
|
||||||
|
case device::AgvMapObjectType::ExternalDevice:
|
||||||
|
return msgs::AGV_MAP_OBJECT_EXTERNAL_DEVICE;
|
||||||
|
case device::AgvMapObjectType::Unspecified:
|
||||||
|
default:
|
||||||
|
return msgs::AGV_MAP_OBJECT_UNSPECIFIED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillPose2d(msgs::AgvPose2d* dst, const math::Pose2d& src)
|
||||||
|
{
|
||||||
|
dst->set_x(src.x);
|
||||||
|
dst->set_y(src.y);
|
||||||
|
dst->set_theta(src.theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillVelocity(msgs::AgvVelocity* dst, const device::AgvVelocity& src)
|
||||||
|
{
|
||||||
|
dst->set_vx(src.vx);
|
||||||
|
dst->set_vy(src.vy);
|
||||||
|
dst->set_wz(src.wz);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillBattery(msgs::AgvBatteryState* dst, const device::AgvBatteryState& src)
|
||||||
|
{
|
||||||
|
dst->set_percentage(src.percentage);
|
||||||
|
dst->set_voltage(src.voltage);
|
||||||
|
dst->set_current(src.current);
|
||||||
|
dst->set_temperature(src.temperature);
|
||||||
|
dst->set_charging(src.charging);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillRuntimeState(msgs::AgvRuntimeState* dst, const device::AgvRuntimeState& src)
|
||||||
|
{
|
||||||
|
dst->set_timestamp(src.timestamp);
|
||||||
|
dst->set_mode(static_cast<int>(src.mode));
|
||||||
|
dst->set_connected(src.connected);
|
||||||
|
dst->set_localized(src.localized);
|
||||||
|
dst->set_moving(src.moving);
|
||||||
|
dst->set_fault(src.fault);
|
||||||
|
dst->set_emergency_stopped(src.emergency_stopped);
|
||||||
|
fillPose2d(dst->mutable_pose(), src.pose);
|
||||||
|
fillVelocity(dst->mutable_velocity(), src.velocity);
|
||||||
|
fillBattery(dst->mutable_battery(), src.battery);
|
||||||
|
dst->set_current_map(src.current_map);
|
||||||
|
dst->set_current_station(src.current_station);
|
||||||
|
dst->set_last_error(src.last_error);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillNavigationStatus(msgs::AgvNavigationStatus* dst, const device::AgvNavigationStatus& src)
|
||||||
|
{
|
||||||
|
dst->set_state(static_cast<int>(src.state));
|
||||||
|
dst->set_type(static_cast<int>(src.type));
|
||||||
|
dst->set_progress(src.progress);
|
||||||
|
dst->set_message(src.message);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillStation(msgs::AgvStation* dst, const device::AgvStation& src)
|
||||||
|
{
|
||||||
|
dst->set_id(src.id);
|
||||||
|
dst->set_type(src.type);
|
||||||
|
fillPose2d(dst->mutable_pose(), src.pose);
|
||||||
|
dst->set_description(src.description);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillMapPoint3D(msgs::AgvMapPoint3D* dst, const device::AgvMapPoint3D& src)
|
||||||
|
{
|
||||||
|
dst->set_x(src.x);
|
||||||
|
dst->set_y(src.y);
|
||||||
|
dst->set_z(src.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillMapObject(msgs::AgvMapObject* dst, const device::AgvMapObject& src)
|
||||||
|
{
|
||||||
|
dst->set_id(src.id);
|
||||||
|
dst->set_type(toProtoMapObjectType(src.type));
|
||||||
|
for (const auto& point : src.points) {
|
||||||
|
fillMapPoint3D(dst->add_points(), point);
|
||||||
|
}
|
||||||
|
dst->set_heading(src.heading);
|
||||||
|
auto* properties = dst->mutable_properties();
|
||||||
|
for (const auto& [key, value] : src.properties) {
|
||||||
|
(*properties)[key] = value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillUnifiedMap2D(msgs::AgvUnifiedMap2D* dst, const device::AgvUnifiedMap2D& src)
|
||||||
|
{
|
||||||
|
dst->set_frame_id(src.frame_id);
|
||||||
|
dst->set_timestamp(src.timestamp);
|
||||||
|
dst->set_resolution(src.resolution);
|
||||||
|
dst->set_width(src.width);
|
||||||
|
dst->set_height(src.height);
|
||||||
|
fillPose2d(dst->mutable_origin(), src.origin);
|
||||||
|
for (const auto value : src.data) {
|
||||||
|
dst->add_data(value);
|
||||||
|
}
|
||||||
|
for (const auto& object : src.objects) {
|
||||||
|
fillMapObject(dst->add_objects(), object);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillUnifiedMap3D(msgs::AgvUnifiedMap3D* dst, const device::AgvUnifiedMap3D& src)
|
||||||
|
{
|
||||||
|
dst->set_frame_id(src.frame_id);
|
||||||
|
dst->set_timestamp(src.timestamp);
|
||||||
|
dst->set_voxel_resolution(src.voxel_resolution);
|
||||||
|
for (const auto& point : src.points) {
|
||||||
|
auto* dst_point = dst->add_points();
|
||||||
|
dst_point->set_x(point.x);
|
||||||
|
dst_point->set_y(point.y);
|
||||||
|
dst_point->set_z(point.z);
|
||||||
|
dst_point->set_intensity(point.intensity);
|
||||||
|
dst_point->set_ring(point.ring);
|
||||||
|
dst_point->set_time_offset(point.time_offset);
|
||||||
|
}
|
||||||
|
for (const auto& voxel : src.voxels) {
|
||||||
|
auto* dst_voxel = dst->add_voxels();
|
||||||
|
dst_voxel->set_x(voxel.x);
|
||||||
|
dst_voxel->set_y(voxel.y);
|
||||||
|
dst_voxel->set_z(voxel.z);
|
||||||
|
dst_voxel->set_probability(voxel.probability);
|
||||||
|
}
|
||||||
|
for (const auto& plane : src.planes) {
|
||||||
|
auto* dst_plane = dst->add_planes();
|
||||||
|
fillMapPoint3D(dst_plane->mutable_center(), plane.center);
|
||||||
|
fillMapPoint3D(dst_plane->mutable_normal(), plane.normal);
|
||||||
|
dst_plane->set_d(plane.d);
|
||||||
|
dst_plane->set_radius(plane.radius);
|
||||||
|
}
|
||||||
|
for (const auto& object : src.objects) {
|
||||||
|
fillMapObject(dst->add_objects(), object);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void fillUnifiedMapUpdate(msgs::AgvUnifiedMapUpdate* dst, const device::AgvUnifiedMapUpdate& src)
|
||||||
|
{
|
||||||
|
dst->set_map_id(src.map_id);
|
||||||
|
dst->set_session_id(src.session_id);
|
||||||
|
dst->set_sequence(src.sequence);
|
||||||
|
dst->set_resume_token(src.resume_token);
|
||||||
|
dst->set_dimension(toProtoMapDimension(src.dimension));
|
||||||
|
dst->set_update_type(toProtoMapUpdateType(src.update_type));
|
||||||
|
dst->set_frame_id(src.frame_id);
|
||||||
|
dst->set_timestamp(src.timestamp);
|
||||||
|
dst->set_snapshot_begin(src.snapshot_begin);
|
||||||
|
dst->set_snapshot_end(src.snapshot_end);
|
||||||
|
dst->set_chunk_index(src.chunk_index);
|
||||||
|
dst->set_chunk_count(src.chunk_count);
|
||||||
|
if (src.map_2d) {
|
||||||
|
fillUnifiedMap2D(dst->mutable_map_2d(), *src.map_2d);
|
||||||
|
} else if (src.map_3d) {
|
||||||
|
fillUnifiedMap3D(dst->mutable_map_3d(), *src.map_3d);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
gRPCAgvServiceImpl::gRPCAgvServiceImpl()
|
||||||
|
: dmgr_(device::DeviceManager::getInstance())
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*,
|
||||||
|
const api::AgvRuntimeStateCommand_Request* request,
|
||||||
|
api::AgvRuntimeStateCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
fillRuntimeState(response->mutable_state(), agv->runtimeState());
|
||||||
|
fillFeedback(response->mutable_header(), true);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*,
|
||||||
|
const api::AgvNavigationStatusCommand_Request* request,
|
||||||
|
api::AgvNavigationStatusCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
fillNavigationStatus(response->mutable_status(), agv->navigationStatus());
|
||||||
|
fillFeedback(response->mutable_header(), true);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->emergencyStop());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->clearFault());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*,
|
||||||
|
const api::AgvNavigateToPoseCommand_Request* request,
|
||||||
|
api::AgvNavigateToPoseCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->navigateToPose(
|
||||||
|
toPose2d(request->pose()),
|
||||||
|
toMotionOptions(request->options()),
|
||||||
|
toAdapterParams(request->adapter_params())));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*,
|
||||||
|
const api::AgvNavigateToStationCommand_Request* request,
|
||||||
|
api::AgvNavigateToStationCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->navigateToStation(
|
||||||
|
request->station_id(),
|
||||||
|
toMotionOptions(request->options()),
|
||||||
|
toAdapterParams(request->adapter_params())));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*,
|
||||||
|
const api::AgvFollowPathCommand_Request* request,
|
||||||
|
api::AgvFollowPathCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
std::vector<device::AgvPathSegment> path;
|
||||||
|
path.reserve(static_cast<std::size_t>(request->path_size()));
|
||||||
|
for (const auto& segment : request->path()) {
|
||||||
|
path.push_back(toPathSegment(segment));
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->followPath(path));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->pauseNavigation());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->resumeNavigation());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->cancelNavigation());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*,
|
||||||
|
const api::AgvSetVelocityCommand_Request* request,
|
||||||
|
api::AgvSetVelocityCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity())));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->stopVelocityControl());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*,
|
||||||
|
const api::AgvListMapsCommand_Request* request,
|
||||||
|
api::AgvListMapsCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
std::vector<std::string> maps;
|
||||||
|
const auto result = agv->listMaps(maps);
|
||||||
|
if (result.ok()) {
|
||||||
|
for (const auto& map : maps) {
|
||||||
|
response->add_maps(map);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return setResponseResult(response, result);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*,
|
||||||
|
const api::AgvListStationsCommand_Request* request,
|
||||||
|
api::AgvListStationsCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
std::vector<device::AgvStation> stations;
|
||||||
|
const auto result = agv->listStations(stations);
|
||||||
|
if (result.ok()) {
|
||||||
|
for (const auto& station : stations) {
|
||||||
|
fillStation(response->add_stations(), station);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return setResponseResult(response, result);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->switchMap(request->map_name()));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->uploadMap(request->map_name(), request->content()));
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*,
|
||||||
|
const api::AgvMapCommand_Request* request,
|
||||||
|
api::AgvMapCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
std::string content;
|
||||||
|
const auto result = agv->downloadMap(request->map_name(), content);
|
||||||
|
if (result.ok()) {
|
||||||
|
response->set_content(content);
|
||||||
|
}
|
||||||
|
return setResponseResult(response, result);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*,
|
||||||
|
const api::AgvStartMappingCommand_Request* request,
|
||||||
|
api::AgvStartMappingCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
device::AgvMappingOptions options;
|
||||||
|
options.dimension = toMapDimension(request->dimension());
|
||||||
|
options.map_name = request->map_name();
|
||||||
|
options.real_time = request->real_time();
|
||||||
|
const auto result = agv->startMapping(options);
|
||||||
|
if (result.ok()) {
|
||||||
|
response->set_session_id(device_id + "_mapping");
|
||||||
|
}
|
||||||
|
return setResponseResult(response, result);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response->mutable_header(), false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context,
|
||||||
|
const api::AgvMapStreamCommand_Request* request,
|
||||||
|
grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->header().device_id();
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||||
|
|
||||||
|
api::AgvMapStreamCommand_Feedback feedback;
|
||||||
|
if (!agv) {
|
||||||
|
const std::string message = "AGV device not found: " + device_id;
|
||||||
|
fillFeedback(feedback.mutable_header(), false, message);
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||||
|
}
|
||||||
|
|
||||||
|
device::AgvMapStreamOptions options;
|
||||||
|
options.dimension = toMapDimension(request->dimension());
|
||||||
|
options.map_name = request->map_name();
|
||||||
|
options.resume_token = request->resume_token();
|
||||||
|
options.snapshot = request->snapshot();
|
||||||
|
options.incremental = request->incremental();
|
||||||
|
options.max_chunk_bytes = request->max_chunk_bytes();
|
||||||
|
|
||||||
|
std::uint64_t after_sequence = 0;
|
||||||
|
if (!request->resume_token().empty()) {
|
||||||
|
try {
|
||||||
|
after_sequence = static_cast<std::uint64_t>(std::stoull(request->resume_token()));
|
||||||
|
} catch (...) {
|
||||||
|
after_sequence = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool wrote_any = false;
|
||||||
|
while (!context->IsCancelled()) {
|
||||||
|
options.wait_timeout_ms = (!options.incremental && wrote_any) ? 20 : 1000;
|
||||||
|
device::AgvUnifiedMapUpdate update;
|
||||||
|
const auto result = agv->getUnifiedMapUpdate(after_sequence, options, update);
|
||||||
|
if (!result.ok()) {
|
||||||
|
if (result.code == device::AgvErrorCode::Timeout && wrote_any && !options.incremental) {
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
if (result.code == device::AgvErrorCode::Timeout && wrote_any && options.incremental) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
fillFeedback(feedback.mutable_header(), false, result.message);
|
||||||
|
writer->Write(feedback);
|
||||||
|
return resultToStatus(result);
|
||||||
|
}
|
||||||
|
|
||||||
|
api::AgvMapStreamCommand_Feedback update_feedback;
|
||||||
|
fillFeedback(update_feedback.mutable_header(), true);
|
||||||
|
fillUnifiedMapUpdate(update_feedback.mutable_update(), update);
|
||||||
|
if (!writer->Write(update_feedback)) {
|
||||||
|
return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream writer closed");
|
||||||
|
}
|
||||||
|
wrote_any = true;
|
||||||
|
after_sequence = update.sequence;
|
||||||
|
options.resume_token.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream cancelled");
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
api::AgvMapStreamCommand_Feedback feedback;
|
||||||
|
fillFeedback(feedback.mutable_header(), false, e.what());
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*,
|
||||||
|
const api::CommandHeader_Request* request,
|
||||||
|
api::CommandHeader_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||||
|
if (!agv) {
|
||||||
|
return setDeviceNotFound(response, request->device_id());
|
||||||
|
}
|
||||||
|
return setResponseResult(response, agv->stopMapping());
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::service
|
||||||
@ -407,4 +407,23 @@ grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*,
|
|||||||
return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented");
|
return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
|
||||||
|
const cmvr::api::CommandHeader_Request *request,
|
||||||
|
cmvr::api::CommandHeader_Feedback *response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
const std::string device_id = request->device_id();
|
||||||
|
auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
|
||||||
|
if (!arm) {
|
||||||
|
return setDeviceNotFound(response, device_id);
|
||||||
|
}
|
||||||
|
const auto result = arm->clearFault();
|
||||||
|
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
|
||||||
|
return resultToStatus(result);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
fillFeedback(response, false, e.what());
|
||||||
|
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||||
|
}
|
||||||
|
}
|
||||||
} // namespace cmvr::service
|
} // namespace cmvr::service
|
||||||
|
|
||||||
|
|||||||
@ -1,10 +1,14 @@
|
|||||||
#include "common/base/logging/logger.h"
|
#include "common/base/logging/logger.h"
|
||||||
|
#include "manager/media_source_hub/include/device_media_source_adapter.h"
|
||||||
//
|
//
|
||||||
// Created by xtkuang on 2025/6/1.
|
// Created by xtkuang on 2025/6/1.
|
||||||
//
|
//
|
||||||
|
|
||||||
#include "../include/grpc_camera_service.h"
|
#include "../include/grpc_camera_service.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
@ -20,9 +24,88 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
|
|||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out)
|
||||||
|
{
|
||||||
|
switch (command) {
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_TILT_UP:
|
||||||
|
out = PtzCommand::TiltUp;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_TILT_DOWN:
|
||||||
|
out = PtzCommand::TiltDown;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_PAN_LEFT:
|
||||||
|
out = PtzCommand::PanLeft;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_PAN_RIGHT:
|
||||||
|
out = PtzCommand::PanRight;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_UP_LEFT:
|
||||||
|
out = PtzCommand::UpLeft;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_UP_RIGHT:
|
||||||
|
out = PtzCommand::UpRight;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_DOWN_LEFT:
|
||||||
|
out = PtzCommand::DownLeft;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_DOWN_RIGHT:
|
||||||
|
out = PtzCommand::DownRight;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_ZOOM_IN:
|
||||||
|
out = PtzCommand::ZoomIn;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_ZOOM_OUT:
|
||||||
|
out = PtzCommand::ZoomOut;
|
||||||
|
return true;
|
||||||
|
case cmvr::api::ControlPtzCommand_Command_PAN_AUTO:
|
||||||
|
out = PtzCommand::PanAuto;
|
||||||
|
return true;
|
||||||
|
default:
|
||||||
|
return false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
gRPCCameraServiceImpl::gRPCCameraServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
|
// The legacy depth/RGBD RPCs acquire the camera's shared producer directly
|
||||||
|
// instead of going through MediaSourceHub. Keep that lease exception-safe:
|
||||||
|
// cancellation, a failed Write(), or any conversion error must release exactly
|
||||||
|
// the one startStreaming() reference acquired by this call.
|
||||||
|
class CameraStreamingLease final {
|
||||||
|
public:
|
||||||
|
explicit CameraStreamingLease(std::shared_ptr<AbstractCamera> camera)
|
||||||
|
: camera_(std::move(camera)) {
|
||||||
|
active_ = camera_ && camera_->startStreaming();
|
||||||
|
}
|
||||||
|
|
||||||
|
~CameraStreamingLease() {
|
||||||
|
if (!active_ || !camera_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
camera_->stopStreaming();
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease: "
|
||||||
|
<< e.what();
|
||||||
|
} catch (...) {
|
||||||
|
CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
CameraStreamingLease(const CameraStreamingLease&) = delete;
|
||||||
|
CameraStreamingLease& operator=(const CameraStreamingLease&) = delete;
|
||||||
|
|
||||||
|
explicit operator bool() const noexcept { return active_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<AbstractCamera> camera_;
|
||||||
|
bool active_{false};
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
gRPCCameraServiceImpl::gRPCCameraServiceImpl(
|
||||||
|
CameraStreamLowLatencyConfig stream_config)
|
||||||
|
: dmgr_(DeviceManager::getInstance()),
|
||||||
|
stream_config_(stream_config) {}
|
||||||
|
|
||||||
grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
|
grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
|
||||||
const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response)
|
const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response)
|
||||||
@ -47,14 +130,6 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
|
|||||||
response->mutable_state()->set_fps(state.fps);
|
response->mutable_state()->set_fps(state.fps);
|
||||||
response->mutable_state()->set_width(state.width);
|
response->mutable_state()->set_width(state.width);
|
||||||
response->mutable_state()->set_height(state.height);
|
response->mutable_state()->set_height(state.height);
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetStatus): success, id=" << dev_id
|
|
||||||
<< ", initialized=" << state.is_initialized
|
|
||||||
<< ", opened=" << state.is_opened
|
|
||||||
<< ", streaming=" << state.is_streaming
|
|
||||||
<< ", recording=" << state.is_recording
|
|
||||||
<< ", error=" << state.is_error
|
|
||||||
<< ", size=" << state.width << "x" << state.height
|
|
||||||
<< ", fps=" << state.fps;
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch(const exception &e) {
|
catch(const exception &e) {
|
||||||
@ -80,7 +155,6 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
|
|||||||
}
|
}
|
||||||
response->mutable_header()->set_success(true);
|
response->mutable_header()->set_success(true);
|
||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartCamera): success, id=" << dev_id;
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (const exception &e) {
|
catch (const exception &e) {
|
||||||
@ -106,7 +180,6 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
|
|||||||
}
|
}
|
||||||
response->mutable_header()->set_success(true);
|
response->mutable_header()->set_success(true);
|
||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopCamera): success, id=" << dev_id;
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -160,10 +233,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
|
|||||||
response->mutable_color_frame()->set_height(image.rows);
|
response->mutable_color_frame()->set_height(image.rows);
|
||||||
response->mutable_color_frame()->set_width(image.cols);
|
response->mutable_color_frame()->set_width(image.cols);
|
||||||
response->mutable_color_frame()->set_codec("none");
|
response->mutable_color_frame()->set_codec("none");
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImage): success, id=" << dev_id
|
|
||||||
<< ", size=" << image.cols << "x" << image.rows
|
|
||||||
<< ", cv_type=" << image.type()
|
|
||||||
<< ", bytes=" << image.total() * image.elemSize();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -220,10 +289,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
|
|||||||
response->mutable_depth_frame()->set_height(image.rows);
|
response->mutable_depth_frame()->set_height(image.rows);
|
||||||
response->mutable_depth_frame()->set_width(image.cols);
|
response->mutable_depth_frame()->set_width(image.cols);
|
||||||
response->mutable_depth_frame()->set_codec("none");
|
response->mutable_depth_frame()->set_codec("none");
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImage): success, id=" << dev_id
|
|
||||||
<< ", size=" << image.cols << "x" << image.rows
|
|
||||||
<< ", cv_type=" << image.type()
|
|
||||||
<< ", bytes=" << image.total() * image.elemSize();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -293,11 +358,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
|
|||||||
response->mutable_depth_frame()->set_height(depth_image.rows);
|
response->mutable_depth_frame()->set_height(depth_image.rows);
|
||||||
response->mutable_depth_frame()->set_width(depth_image.cols);
|
response->mutable_depth_frame()->set_width(depth_image.cols);
|
||||||
response->mutable_depth_frame()->set_codec("none");
|
response->mutable_depth_frame()->set_codec("none");
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImages): success, id=" << dev_id
|
|
||||||
<< ", color_size=" << color_image.cols << "x" << color_image.rows
|
|
||||||
<< ", color_type=" << color_image.type()
|
|
||||||
<< ", depth_size=" << depth_image.cols << "x" << depth_image.rows
|
|
||||||
<< ", depth_type=" << depth_image.type();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -321,8 +381,6 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
|
|||||||
dev->startRecording(request->video_path());
|
dev->startRecording(request->video_path());
|
||||||
response->mutable_header()->set_success(true);
|
response->mutable_header()->set_success(true);
|
||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartRecording): success, id=" << dev_id
|
|
||||||
<< ", path=" << request->video_path();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -346,7 +404,50 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
|
|||||||
dev->stopRecording();
|
dev->stopRecording();
|
||||||
response->mutable_header()->set_success(true);
|
response->mutable_header()->set_success(true);
|
||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id;
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
catch (exception &e) {
|
||||||
|
response->mutable_header()->set_success(false);
|
||||||
|
response->mutable_header()->set_error_message(e.what());
|
||||||
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
|
||||||
|
const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response)
|
||||||
|
{
|
||||||
|
try {
|
||||||
|
string dev_id = request->header().device_id();
|
||||||
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id
|
||||||
|
<< ", command=" << request->command()
|
||||||
|
<< ", action=" << request->action()
|
||||||
|
<< ", speed=" << request->speed();
|
||||||
|
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
||||||
|
if (!dev) {
|
||||||
|
return failResponse(response, "Camera device not found: " + dev_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
PtzCommand command{};
|
||||||
|
if (!toPtzCommand(request->command(), command)) {
|
||||||
|
return failResponse(response, "Invalid PTZ command");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request->action() != api::ControlPtzCommand_Action_START &&
|
||||||
|
request->action() != api::ControlPtzCommand_Action_STOP) {
|
||||||
|
return failResponse(response, "Invalid PTZ action");
|
||||||
|
}
|
||||||
|
const bool stop = request->action() == api::ControlPtzCommand_Action_STOP;
|
||||||
|
if (!dev->controlPtz(command, stop, static_cast<int>(request->speed()))) {
|
||||||
|
CameraState state{};
|
||||||
|
dev->getState(state);
|
||||||
|
const std::string error_message =
|
||||||
|
state.error_message.empty() ? "Failed to control PTZ: " + dev_id : state.error_message;
|
||||||
|
return failResponse(response, error_message);
|
||||||
|
}
|
||||||
|
|
||||||
|
response->mutable_header()->set_success(true);
|
||||||
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (exception &e) {
|
||||||
@ -363,9 +464,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
|
|||||||
try {
|
try {
|
||||||
//读取首次传递的数据，获取设备id
|
//读取首次传递的数据，获取设备id
|
||||||
api::GetDepthImageStreamCommand_Request request;
|
api::GetDepthImageStreamCommand_Request request;
|
||||||
stream->Read(&request);
|
if (!stream->Read(&request)) {
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
string dev_id = request.header().device_id();
|
string dev_id = request.header().device_id();
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id;
|
||||||
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
||||||
if (!dev) {
|
if (!dev) {
|
||||||
api::GetDepthImageStreamCommand_Feedback response;
|
api::GetDepthImageStreamCommand_Feedback response;
|
||||||
@ -375,9 +478,17 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
|
|||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
|
CameraStreamingLease stream_lease(dev);
|
||||||
|
if (!stream_lease) {
|
||||||
|
api::GetDepthImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
|
response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
|
stream->Write(response);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
int nFrameCount = 0;
|
int nFrameCount = 0;
|
||||||
dev->startStreaming();
|
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start streaming success, id=" << dev_id;
|
|
||||||
size_t index = 0;
|
size_t index = 0;
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
@ -389,9 +500,10 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
|
|||||||
|
|
||||||
api::GetDepthImageStreamCommand_Feedback response;
|
api::GetDepthImageStreamCommand_Feedback response;
|
||||||
cmvr::device::StreamFrameData frame_data;
|
cmvr::device::StreamFrameData frame_data;
|
||||||
dev->getEncodedFrame(frame_data,index);
|
if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
|
||||||
if (!frame_data.rgbFrame.empty()) {
|
!frame_data.depthFrame.empty()) {
|
||||||
response.mutable_header()->set_success(true);
|
response.mutable_header()->set_success(true);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
|
response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
|
||||||
response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
|
response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
|
||||||
response.mutable_depth_frame()->set_codec(frame_data.codec);
|
response.mutable_depth_frame()->set_codec(frame_data.codec);
|
||||||
@ -399,29 +511,27 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
|
|||||||
response.mutable_depth_frame()->set_height(frame_data.height);
|
response.mutable_depth_frame()->set_height(frame_data.height);
|
||||||
|
|
||||||
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
|
||||||
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
|
||||||
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
|
||||||
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
|
||||||
for (int i = 0; i < 5 ; i++) {
|
for (int i = 0; i < 5 ; i++) {
|
||||||
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
|
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
response.set_seq_no(nFrameCount++);
|
response.set_seq_no(nFrameCount++);
|
||||||
|
|
||||||
grpc::WriteOptions options;
|
|
||||||
options.set_last_message();
|
|
||||||
if (!stream->Write(response)) {
|
if (!stream->Write(response)) {
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id;
|
||||||
dev->stopStreaming();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (const exception &e) {
|
||||||
api::GetDepthImageStreamCommand_Feedback response;
|
api::GetDepthImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
response.mutable_header()->set_error_message(e.what());
|
response.mutable_header()->set_error_message(e.what());
|
||||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
@ -433,9 +543,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
|
|||||||
try {
|
try {
|
||||||
//读取首次传递的数据，获取设备id
|
//读取首次传递的数据，获取设备id
|
||||||
api::GetRGBDImagesStreamCommand_Request request;
|
api::GetRGBDImagesStreamCommand_Request request;
|
||||||
stream->Read(&request);
|
if (!stream->Read(&request)) {
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
string dev_id = request.header().device_id();
|
string dev_id = request.header().device_id();
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id;
|
||||||
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
||||||
if (!dev) {
|
if (!dev) {
|
||||||
api::GetRGBDImagesStreamCommand_Feedback response;
|
api::GetRGBDImagesStreamCommand_Feedback response;
|
||||||
@ -445,9 +557,17 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
|
|||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
|
CameraStreamingLease stream_lease(dev);
|
||||||
|
if (!stream_lease) {
|
||||||
|
api::GetRGBDImagesStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
|
response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
|
stream->Write(response);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
int nFrameCount = 0;
|
int nFrameCount = 0;
|
||||||
dev->startStreaming();
|
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id;
|
|
||||||
size_t index = 0;
|
size_t index = 0;
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
@ -459,9 +579,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
|
|||||||
|
|
||||||
api::GetRGBDImagesStreamCommand_Feedback response;
|
api::GetRGBDImagesStreamCommand_Feedback response;
|
||||||
cmvr::device::StreamFrameData frame_data;
|
cmvr::device::StreamFrameData frame_data;
|
||||||
dev->getEncodedFrame(frame_data,index);
|
if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
|
||||||
if (!frame_data.rgbFrame.empty()) {
|
!frame_data.rgbFrame.empty() &&
|
||||||
|
!frame_data.depthFrame.empty()) {
|
||||||
response.mutable_header()->set_success(true);
|
response.mutable_header()->set_success(true);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
|
response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
|
||||||
response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
|
response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
|
||||||
response.mutable_color_frame()->set_codec(frame_data.codec);
|
response.mutable_color_frame()->set_codec(frame_data.codec);
|
||||||
@ -475,17 +597,15 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
|
|||||||
response.mutable_depth_frame()->set_height(frame_data.height);
|
response.mutable_depth_frame()->set_height(frame_data.height);
|
||||||
|
|
||||||
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
|
||||||
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
|
||||||
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
|
||||||
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
|
||||||
for (int i = 0; i < 5 ; i++) {
|
for (int i = 0; i < 5 ; i++) {
|
||||||
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
|
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
response.set_seq_no(nFrameCount++);
|
response.set_seq_no(nFrameCount++);
|
||||||
|
|
||||||
grpc::WriteOptions options;
|
|
||||||
options.set_last_message();
|
|
||||||
if (!stream->Write(response)) {
|
if (!stream->Write(response)) {
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
||||||
break;
|
break;
|
||||||
@ -493,11 +613,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
|
||||||
dev->stopStreaming();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (const exception &e) {
|
||||||
api::GetRGBDImagesStreamCommand_Feedback response;
|
api::GetRGBDImagesStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
response.mutable_header()->set_error_message(e.what());
|
response.mutable_header()->set_error_message(e.what());
|
||||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
@ -508,9 +628,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
|
|||||||
try {
|
try {
|
||||||
//读取首次传递的数据，获取设备id
|
//读取首次传递的数据，获取设备id
|
||||||
api::GetRGBImageStreamCommand_Request request;
|
api::GetRGBImageStreamCommand_Request request;
|
||||||
stream->Read(&request);
|
if (!stream->Read(&request)) {
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
string dev_id = request.header().device_id();
|
string dev_id = request.header().device_id();
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id
|
||||||
|
<< ", max_pending_frames=" << stream_config_.max_pending_frames
|
||||||
|
<< ", max_frame_age_ms=" << stream_config_.max_frame_age.count();
|
||||||
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
|
||||||
if (!dev) {
|
if (!dev) {
|
||||||
api::GetRGBImageStreamCommand_Feedback response;
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
@ -520,10 +644,73 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
|
|||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
int nFrameCount = 0;
|
auto& media_hub = cmvr::media::globalMediaSourceHub();
|
||||||
dev->startStreaming();
|
const std::string track_id = cmvr::media::cameraColorTrackId(dev_id);
|
||||||
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id;
|
if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) {
|
||||||
size_t last_sent_index = std::numeric_limits<size_t>::max();
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
|
response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
|
stream->Write(response);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
auto subscription = media_hub.subscribe(
|
||||||
|
track_id,
|
||||||
|
cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
|
||||||
|
[context] { return context->IsCancelled(); });
|
||||||
|
if (!subscription) {
|
||||||
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
|
response.mutable_header()->set_error_message("Failed to subscribe camera media source: " + dev_id);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
|
stream->Write(response);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool waiting_for_key_frame = true;
|
||||||
|
auto last_key_frame_request = std::chrono::steady_clock::now();
|
||||||
|
auto last_latency_log = std::chrono::steady_clock::time_point{};
|
||||||
|
uint64_t discarded_since_log = 0;
|
||||||
|
std::chrono::microseconds last_write_duration{0};
|
||||||
|
media_hub.requestKeyFrame(track_id);
|
||||||
|
const auto request_key_frame_if_due = [&] {
|
||||||
|
const auto now = std::chrono::steady_clock::now();
|
||||||
|
if (now - last_key_frame_request >= std::chrono::milliseconds(500)) {
|
||||||
|
media_hub.requestKeyFrame(track_id);
|
||||||
|
last_key_frame_request = now;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
const auto request_key_frame_now = [&] {
|
||||||
|
waiting_for_key_frame = true;
|
||||||
|
media_hub.requestKeyFrame(track_id);
|
||||||
|
last_key_frame_request = std::chrono::steady_clock::now();
|
||||||
|
};
|
||||||
|
const auto log_latency_event = [&](
|
||||||
|
const char* reason,
|
||||||
|
const uint64_t discarded,
|
||||||
|
const uint64_t frame_age_ns,
|
||||||
|
const std::chrono::microseconds write_duration) {
|
||||||
|
discarded_since_log += discarded;
|
||||||
|
const auto now = std::chrono::steady_clock::now();
|
||||||
|
if (last_latency_log != std::chrono::steady_clock::time_point{} &&
|
||||||
|
now - last_latency_log < std::chrono::seconds(1)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
const double frame_age_ms = static_cast<double>(frame_age_ns) / 1'000'000.0;
|
||||||
|
const double write_ms = static_cast<double>(write_duration.count()) / 1'000.0;
|
||||||
|
CMVR_LOG(WARNING) << "[gRPCCameraServiceImpl] low-latency camera stream event"
|
||||||
|
<< ", id=" << dev_id
|
||||||
|
<< ", reason=" << reason
|
||||||
|
<< ", discarded=" << discarded_since_log
|
||||||
|
<< ", age_ms=" << frame_age_ms
|
||||||
|
<< ", write_ms=" << write_ms
|
||||||
|
<< ", max_pending_frames="
|
||||||
|
<< stream_config_.max_pending_frames
|
||||||
|
<< ", max_frame_age_ms="
|
||||||
|
<< stream_config_.max_frame_age.count();
|
||||||
|
discarded_since_log = 0;
|
||||||
|
last_latency_log = now;
|
||||||
|
};
|
||||||
while (true)
|
while (true)
|
||||||
{
|
{
|
||||||
if (context->IsCancelled())
|
if (context->IsCancelled())
|
||||||
@ -532,45 +719,122 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
api::GetRGBImageStreamCommand_Feedback response;
|
const auto read = subscription.waitRead(std::chrono::milliseconds(100));
|
||||||
cmvr::device::StreamFrameData frame_data;
|
if (!read || !read->value || read->value->empty()) {
|
||||||
size_t next_index = last_sent_index;
|
if (!subscription.valid()) {
|
||||||
if (!dev->getLatestEncodedFrame(frame_data, next_index) ||
|
break;
|
||||||
frame_data.rgbFrame.empty() ||
|
}
|
||||||
next_index == last_sent_index) {
|
if (waiting_for_key_frame) {
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
request_key_frame_if_due();
|
||||||
|
}
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
const auto& frame = *read->value;
|
||||||
|
const auto descriptor = frame.descriptor;
|
||||||
|
if (!descriptor) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const uint64_t now_ns = static_cast<uint64_t>(
|
||||||
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::steady_clock::now().time_since_epoch()).count());
|
||||||
|
const auto frame_age = cameraFrameAgeNs(frame.capture_time_ns, now_ns);
|
||||||
|
if (frame_age &&
|
||||||
|
cameraFrameExceedsAgeLimit(
|
||||||
|
frame.capture_time_ns,
|
||||||
|
now_ns,
|
||||||
|
stream_config_.max_frame_age)) {
|
||||||
|
// This frame is already outside the latency budget. Flush all
|
||||||
|
// currently queued frames and wait for a fresh IDR; sending any
|
||||||
|
// P/B frame after an intentional gap would break decoder continuity.
|
||||||
|
const uint64_t discarded =
|
||||||
|
1 + subscription.discardPendingIfExceeds(0);
|
||||||
|
request_key_frame_now();
|
||||||
|
log_latency_event(
|
||||||
|
"stale_frame",
|
||||||
|
discarded,
|
||||||
|
*frame_age,
|
||||||
|
last_write_duration);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const bool inter_frame_codec = descriptor->codec == cmvr::media::Codec::H264 ||
|
||||||
|
descriptor->codec == cmvr::media::Codec::H265;
|
||||||
|
if (!inter_frame_codec || descriptor->payload_format != cmvr::media::PayloadFormat::ANNEX_B) {
|
||||||
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
|
response.mutable_header()->set_error_message(
|
||||||
|
"Unsupported camera stream codec or payload format: " + dev_id);
|
||||||
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
|
stream->Write(response);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) {
|
||||||
|
request_key_frame_now();
|
||||||
|
}
|
||||||
|
if (waiting_for_key_frame && !frame.key_frame) {
|
||||||
|
request_key_frame_if_due();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
waiting_for_key_frame = false;
|
||||||
|
|
||||||
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
response.mutable_header()->set_success(true);
|
response.mutable_header()->set_success(true);
|
||||||
response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
|
response.mutable_color_frame()->set_data(frame.data(), frame.size());
|
||||||
response.mutable_color_frame()->set_codec(frame_data.codec);
|
response.mutable_color_frame()->set_is_key_frame(frame.key_frame);
|
||||||
response.mutable_color_frame()->set_width(frame_data.width);
|
response.mutable_color_frame()->set_codec(
|
||||||
response.mutable_color_frame()->set_height(frame_data.height);
|
descriptor->codec == cmvr::media::Codec::H264 ? "h264" :
|
||||||
|
descriptor->codec == cmvr::media::Codec::H265 ? "h265" : "unknown");
|
||||||
|
response.mutable_color_frame()->set_width(static_cast<int32_t>(descriptor->width));
|
||||||
|
response.mutable_color_frame()->set_height(static_cast<int32_t>(descriptor->height));
|
||||||
|
response.mutable_color_frame()->set_capture_utc_ns(frame.capture_utc_ns);
|
||||||
|
response.mutable_color_frame()->set_source_sequence(frame.sequence);
|
||||||
|
response.mutable_color_frame()->set_pts(frame.pts);
|
||||||
|
response.mutable_color_frame()->set_dts(frame.dts);
|
||||||
|
response.mutable_color_frame()->set_source_fps(descriptor->nominal_rate);
|
||||||
|
response.mutable_color_frame()->set_source_timestamp(frame.source_timestamp);
|
||||||
|
response.mutable_color_frame()->set_source_frame_number(frame.source_frame_number);
|
||||||
|
|
||||||
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
|
response.mutable_intrinsics()->set_fx(descriptor->fx);
|
||||||
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
|
response.mutable_intrinsics()->set_fy(descriptor->fy);
|
||||||
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
|
response.mutable_intrinsics()->set_cx(descriptor->cx);
|
||||||
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
|
response.mutable_intrinsics()->set_cy(descriptor->cy);
|
||||||
for (int i = 0; i < 5 ; i++) {
|
for (const float coefficient : descriptor->distortion) {
|
||||||
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
|
response.mutable_intrinsics()->add_coeffs(coefficient);
|
||||||
}
|
}
|
||||||
|
|
||||||
response.set_seq_no(nFrameCount++);
|
response.set_seq_no(static_cast<int32_t>(std::min<uint64_t>(
|
||||||
|
frame.sequence,
|
||||||
|
static_cast<uint64_t>(std::numeric_limits<int32_t>::max()))));
|
||||||
|
|
||||||
|
const auto write_started = std::chrono::steady_clock::now();
|
||||||
if (!stream->Write(response)) {
|
if (!stream->Write(response)) {
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
last_sent_index = next_index;
|
last_write_duration = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||||
|
std::chrono::steady_clock::now() - write_started);
|
||||||
|
|
||||||
|
// A successful synchronous Write may have been flow-controlled long
|
||||||
|
// enough for the source to outpace this consumer. Once the pending
|
||||||
|
// count crosses the configured trigger, discard the whole pending
|
||||||
|
// batch and require a fresh key frame before resuming.
|
||||||
|
const uint64_t discarded = subscription.discardPendingIfExceeds(
|
||||||
|
stream_config_.max_pending_frames);
|
||||||
|
if (discarded > 0) {
|
||||||
|
request_key_frame_now();
|
||||||
|
log_latency_event(
|
||||||
|
"write_backpressure",
|
||||||
|
discarded,
|
||||||
|
frame_age.value_or(0),
|
||||||
|
last_write_duration);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
|
CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
|
||||||
dev->stopStreaming();
|
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
catch (exception &e) {
|
catch (const exception &e) {
|
||||||
api::GetRGBImageStreamCommand_Feedback response;
|
api::GetRGBImageStreamCommand_Feedback response;
|
||||||
|
response.mutable_header()->set_success(false);
|
||||||
response.mutable_header()->set_error_message(e.what());
|
response.mutable_header()->set_error_message(e.what());
|
||||||
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
|
||||||
stream->Write(response);
|
stream->Write(response);
|
||||||
|
|||||||
@ -1,4 +1,9 @@
|
|||||||
#include "common/base/logging/logger.h"
|
#include "common/base/logging/logger.h"
|
||||||
|
#include "manager/media_source_hub/include/device_media_source_adapter.h"
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <limits>
|
||||||
//
|
//
|
||||||
// Created by linbo on 2025/6/13.
|
// Created by linbo on 2025/6/13.
|
||||||
// Created by xtkuang on 2025/6/13.
|
// Created by xtkuang on 2025/6/13.
|
||||||
@ -18,6 +23,7 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
|
|||||||
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
|
||||||
return grpc::Status::OK;
|
return grpc::Status::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
|
gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
|
||||||
@ -151,6 +157,111 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context,
|
||||||
|
const api::StreamMicAudioCommand_Request* request,
|
||||||
|
grpc::ServerWriter<api::StreamMicAudioCommand_Feedback>* writer) {
|
||||||
|
try {
|
||||||
|
const string dev_id = request->header().device_id();
|
||||||
|
CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id;
|
||||||
|
const auto dev = dmgr_.getDevice<AbstractMicrophone>(dev_id);
|
||||||
|
if (!dev) {
|
||||||
|
api::StreamMicAudioCommand_Feedback feedback;
|
||||||
|
feedback.mutable_header()->set_success(false);
|
||||||
|
feedback.mutable_header()->set_error_message("Microphone device not found: " + dev_id);
|
||||||
|
setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto& media_hub = cmvr::media::globalMediaSourceHub();
|
||||||
|
const std::string track_id = cmvr::media::microphoneTrackId(dev_id);
|
||||||
|
if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) {
|
||||||
|
api::StreamMicAudioCommand_Feedback feedback;
|
||||||
|
feedback.mutable_header()->set_success(false);
|
||||||
|
feedback.mutable_header()->set_error_message("Failed to register microphone media source: " + dev_id);
|
||||||
|
setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto subscription = media_hub.subscribe(
|
||||||
|
track_id,
|
||||||
|
cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
|
||||||
|
[context] { return context->IsCancelled(); });
|
||||||
|
if (!subscription) {
|
||||||
|
api::StreamMicAudioCommand_Feedback feedback;
|
||||||
|
feedback.mutable_header()->set_success(false);
|
||||||
|
feedback.mutable_header()->set_error_message("Failed to subscribe microphone media source: " + dev_id);
|
||||||
|
setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
while (!context->IsCancelled()) {
|
||||||
|
const auto read = subscription.waitRead(std::chrono::milliseconds(100));
|
||||||
|
if (!read || !read->value || read->value->empty()) {
|
||||||
|
if (!subscription.valid()) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
const auto& frame = *read->value;
|
||||||
|
const auto descriptor = frame.descriptor;
|
||||||
|
if (!descriptor) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
api::StreamMicAudioCommand_Feedback feedback;
|
||||||
|
feedback.mutable_header()->set_success(true);
|
||||||
|
setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
|
||||||
|
auto* audio = feedback.mutable_audio();
|
||||||
|
audio->set_data(frame.data(), frame.size());
|
||||||
|
audio->set_sample_rate(static_cast<int32_t>(std::min<uint32_t>(
|
||||||
|
descriptor->sample_rate,
|
||||||
|
static_cast<uint32_t>(std::numeric_limits<int32_t>::max()))));
|
||||||
|
audio->set_channels(static_cast<int32_t>(std::min<uint32_t>(
|
||||||
|
descriptor->channels,
|
||||||
|
static_cast<uint32_t>(std::numeric_limits<int32_t>::max()))));
|
||||||
|
if (descriptor->codec == cmvr::media::Codec::PCM_S16LE) {
|
||||||
|
audio->set_format(cmvr::api::AudioData_AudioFormat_PCM);
|
||||||
|
audio->set_codec("pcm_s16le");
|
||||||
|
} else if (descriptor->codec == cmvr::media::Codec::OPUS) {
|
||||||
|
audio->set_format(cmvr::api::AudioData_AudioFormat_OPUS);
|
||||||
|
audio->set_codec("opus");
|
||||||
|
} else if (descriptor->codec == cmvr::media::Codec::AAC) {
|
||||||
|
audio->set_format(cmvr::api::AudioData_AudioFormat_AAC);
|
||||||
|
audio->set_codec("aac");
|
||||||
|
} else {
|
||||||
|
feedback.mutable_header()->set_success(false);
|
||||||
|
feedback.mutable_header()->set_error_message(
|
||||||
|
"Unsupported microphone stream codec: " + dev_id);
|
||||||
|
feedback.clear_audio();
|
||||||
|
writer->Write(feedback);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
audio->set_pts(frame.pts);
|
||||||
|
const int64_t sample_count = frame.duration > 0
|
||||||
|
? frame.duration
|
||||||
|
: static_cast<int64_t>(descriptor->nominal_rate);
|
||||||
|
audio->set_nb_samples(static_cast<int32_t>(std::clamp<int64_t>(
|
||||||
|
sample_count,
|
||||||
|
0,
|
||||||
|
std::numeric_limits<int32_t>::max())));
|
||||||
|
if (!writer->Write(feedback)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return grpc::Status::OK;
|
||||||
|
} catch (const std::exception& error) {
|
||||||
|
api::StreamMicAudioCommand_Feedback feedback;
|
||||||
|
feedback.mutable_header()->set_success(false);
|
||||||
|
feedback.mutable_header()->set_error_message(error.what());
|
||||||
|
setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp());
|
||||||
|
writer->Write(feedback);
|
||||||
|
return grpc::Status::OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context,
|
grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context,
|
||||||
const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) {
|
const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) {
|
||||||
try {
|
try {
|
||||||
|
|||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user