feat: extend and refactor SEER Robokit AGV backend
This commit is contained in:
parent
26a7ad5d4b
commit
52d30ee412
@ -2,6 +2,7 @@
|
||||
#define CMVR_ES_AGV_TYPES_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@ -114,7 +115,13 @@ struct AgvMotionOptions {
|
||||
double reach_distance{0.0};
|
||||
double reach_angle{0.0};
|
||||
double speed_ratio{1.0};
|
||||
bool asynchronous{true};
|
||||
// 导航默认同步阻塞;调用方只有显式设为 true 才在任务接受后立即返回。
|
||||
bool asynchronous{false};
|
||||
int wait_timeout_ms{0};
|
||||
int poll_interval_ms{0};
|
||||
// 不带 RPC 框架依赖的取消检查。同步导航等待期间可由
|
||||
// 上层绑定 deadline/cancel;驱动不得在函数返回后保留该回调。
|
||||
std::function<bool()> cancellation_requested;
|
||||
};
|
||||
|
||||
/**
|
||||
@ -220,7 +227,7 @@ struct AgvPathSegment {
|
||||
/**
|
||||
* @brief AGV 扫图过程中产生的数据文件。
|
||||
*
|
||||
* content 可保存控制器返回的二进制内容,例如 SRC1100 的 rawmap zip 包。
|
||||
* content 可保存控制器返回的二进制内容,例如 SEER Robokit 的 rawmap zip 包。
|
||||
*/
|
||||
struct AgvMappingDataFile {
|
||||
std::string name;
|
||||
|
||||
@ -8,8 +8,9 @@ agv {
|
||||
}
|
||||
|
||||
agvs {
|
||||
# 当前部署的控制器型号为 SRC1100;该值是设备实例 ID,不是后端类型名。
|
||||
id: "src1100"
|
||||
src1100_agv {
|
||||
seer_robokit_agv {
|
||||
ip: "192.168.192.5"
|
||||
port_status: 19204
|
||||
port_control: 19205
|
||||
@ -136,9 +136,10 @@ device_manager {
|
||||
}
|
||||
|
||||
devices {
|
||||
# 当前部署的控制器型号为 SRC1100;该值是设备实例 ID,不是后端类型名。
|
||||
id: "src1100"
|
||||
type: DEVICE_TYPE_AGV
|
||||
config_file: "devices/agv/src1100.pb.txt"
|
||||
config_file: "devices/agv/seer_robokit.pb.txt"
|
||||
enable: false
|
||||
}
|
||||
|
||||
|
||||
@ -36,7 +36,7 @@ config/cmvr_es.pb.txt
|
||||
| 大类 | 抽象接口 | 类别工厂 | 当前可选后端 |
|
||||
| --- | --- | --- | --- |
|
||||
| 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 |
|
||||
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit |
|
||||
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME |
|
||||
| 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 |
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
add_subdirectory(my_agv)
|
||||
add_subdirectory(src1100)
|
||||
add_subdirectory(seer_robokit)
|
||||
|
||||
add_library(agv INTERFACE)
|
||||
|
||||
@ -8,7 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(agv
|
||||
INTERFACE
|
||||
cmvr_es::device::my_agv
|
||||
cmvr_es::device::src1100_agv
|
||||
cmvr_es::device::seer_robokit_agv
|
||||
cmvr_es::proto
|
||||
)
|
||||
|
||||
|
||||
@ -85,11 +85,25 @@ public:
|
||||
/**
|
||||
* @brief 发起显式站点到站点路径导航任务。
|
||||
*/
|
||||
virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
|
||||
virtual AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path)
|
||||
{
|
||||
(void)path;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发起显式站点到站点路径导航任务,并指定同步/异步选项。
|
||||
*
|
||||
* 保留单参数虚函数以兼容已有派生类;旧实现会由本重载转发。
|
||||
*/
|
||||
virtual AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path,
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
(void)options;
|
||||
return followPath(path);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 暂停当前导航任务,如果设备支持。
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
#include "devices/agv/my_agv/include/my_agv.h"
|
||||
#include "devices/agv/src1100/include/src1100_agv.h"
|
||||
#include "seer_robokit_agv.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
@ -31,15 +31,15 @@ public:
|
||||
backend.set_id(cfg.id());
|
||||
return std::make_shared<MyAgv>(backend);
|
||||
}
|
||||
case config::AGVDeviceConfig::kSrc1100Agv:
|
||||
case config::AGVDeviceConfig::kSeerRobokitAgv:
|
||||
{
|
||||
if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) {
|
||||
if (!cfg.seer_robokit_agv().id().empty() && cfg.seer_robokit_agv().id() != cfg.id()) {
|
||||
CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id();
|
||||
return nullptr;
|
||||
}
|
||||
auto backend = cfg.src1100_agv();
|
||||
auto backend = cfg.seer_robokit_agv();
|
||||
backend.set_id(cfg.id());
|
||||
return std::make_shared<Src1100Agv>(backend);
|
||||
return std::make_shared<SeerRobokitAgv>(backend);
|
||||
}
|
||||
|
||||
case config::AGVDeviceConfig::BACKEND_NOT_SET:
|
||||
|
||||
57
cmvr-es/devices/agv/seer_robokit/CMakeLists.txt
Normal file
57
cmvr-es/devices/agv/seer_robokit/CMakeLists.txt
Normal file
@ -0,0 +1,57 @@
|
||||
add_library(seer_robokit_agv SHARED
|
||||
src/seer_robokit_agv.cpp
|
||||
src/seer_robokit_transport.cpp
|
||||
src/seer_robokit_control.cpp
|
||||
src/seer_robokit_status.cpp
|
||||
src/seer_robokit_navigation.cpp
|
||||
src/seer_robokit_navigation_wait.cpp
|
||||
src/seer_robokit_map.cpp
|
||||
include/seer_robokit_agv.h
|
||||
include/seer_robokit_protocol.h
|
||||
include/seer_robokit_utils.h
|
||||
include/seer_robokit_navigation_utils.h
|
||||
include/seer_robokit_pgv_utils.h
|
||||
)
|
||||
|
||||
target_include_directories(seer_robokit_agv
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
${PROJECT_SOURCE_DIR}/cmvr-es
|
||||
)
|
||||
|
||||
target_link_libraries(seer_robokit_agv
|
||||
PUBLIC
|
||||
cmvr_es::proto
|
||||
jsoncpp
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv)
|
||||
install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
add_executable(seer_robokit_control_authority_test
|
||||
tests/seer_robokit_control_authority_test.cpp
|
||||
)
|
||||
target_link_libraries(seer_robokit_control_authority_test
|
||||
PRIVATE
|
||||
cmvr_es::device::seer_robokit_agv
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
)
|
||||
add_test(
|
||||
NAME seer_robokit_control_authority_test
|
||||
COMMAND seer_robokit_control_authority_test
|
||||
)
|
||||
set(_seer_robokit_control_authority_test_environment
|
||||
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
|
||||
)
|
||||
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
|
||||
list(APPEND _seer_robokit_control_authority_test_environment
|
||||
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
|
||||
endif()
|
||||
set_tests_properties(seer_robokit_control_authority_test PROPERTIES
|
||||
TIMEOUT 180
|
||||
ENVIRONMENT "${_seer_robokit_control_authority_test_environment}"
|
||||
)
|
||||
endif()
|
||||
394
cmvr-es/devices/agv/seer_robokit/README.md
Normal file
394
cmvr-es/devices/agv/seer_robokit/README.md
Normal file
@ -0,0 +1,394 @@
|
||||
# 仙工 SEER Robokit AGV 适配器
|
||||
|
||||
`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用
|
||||
`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、
|
||||
地图格式转换和错误码解析都封装在本目录内。
|
||||
|
||||
本项目现场使用的控制器型号仍是 SRC1100,所以设备实例 ID 保持为
|
||||
`src1100`;它只用于配置关联和 gRPC 路由,不再作为驱动实现名称。后端配置字段
|
||||
使用 `seer_robokit_agv`,目录、类、库和测试统一使用 `seer_robokit` /
|
||||
`SeerRobokitAgv` 命名。
|
||||
|
||||
从旧版本升级时,外部部署配置必须同步使用 `seer_robokit_agv { ... }`,并把
|
||||
配置路径更新为 `devices/agv/seer_robokit.pb.txt`;设备实例 ID 保持不变。程序、
|
||||
外部配置和部署脚本需要原子升级,不能把旧字段或旧路径与新二进制混用。
|
||||
|
||||
返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
|
||||
|
||||
## 代码与配置
|
||||
|
||||
所有驱动头文件统一放在 `include/`,实现文件统一放在 `src/`;测试源码独立放在
|
||||
`tests/`。除 `seer_robokit_agv.h` 外,其余头文件均为驱动内部实现细节。
|
||||
|
||||
- 公共类声明:[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h)
|
||||
- 导航轮询工具:
|
||||
[`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h)
|
||||
- PGV 参数转换:
|
||||
[`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h)
|
||||
- 协议常量:[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h)
|
||||
- 通用解析工具:[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h)
|
||||
- 生命周期和连接:[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp)
|
||||
- TCP 帧与收发:[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp)
|
||||
- 控制权与受控命令:[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp)
|
||||
- 状态与推送缓存:[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp)
|
||||
- 导航命令:[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp)
|
||||
- 阻塞等待与停车确认:
|
||||
[`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp)
|
||||
- 地图和建图:[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp)
|
||||
- 假控制器测试:
|
||||
[`tests/seer_robokit_control_authority_test.cpp`](tests/seer_robokit_control_authority_test.cpp)
|
||||
- 设备配置:
|
||||
[`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt)
|
||||
- DeviceManager 配置:
|
||||
[`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
|
||||
- gRPC API:
|
||||
[`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、
|
||||
[`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)
|
||||
|
||||
## 配置和启动
|
||||
|
||||
现场配置至少需要修改控制器 IP;端口通常保持仙工默认值:
|
||||
|
||||
```textproto
|
||||
agv {
|
||||
agvs {
|
||||
id: "src1100"
|
||||
seer_robokit_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
|
||||
control_nick_name: "cmvr-es"
|
||||
enable_state_push: true
|
||||
state_push_interval_ms: 200
|
||||
enable_map_update: true
|
||||
map_update_interval_ms: 1000
|
||||
map_update_history_size: 8
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
还要在 `device_manager.pb.txt` 中确认同一个设备 id,并在完成现场安全检查后把
|
||||
`enable` 改为 `true`。源码默认配置故意保持关闭。
|
||||
|
||||
```textproto
|
||||
devices {
|
||||
id: "src1100"
|
||||
type: DEVICE_TYPE_AGV
|
||||
config_file: "devices/agv/seer_robokit.pb.txt"
|
||||
enable: true
|
||||
}
|
||||
```
|
||||
|
||||
构建、安装并启动:
|
||||
|
||||
```bash
|
||||
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
|
||||
cmake --build build -j2
|
||||
cmake --install build
|
||||
./output/bin/cmvr_es
|
||||
```
|
||||
|
||||
`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装,
|
||||
或通过程序支持的外部配置入口启动,不能只修改源码文件后继续使用旧的
|
||||
`output/` 配置。
|
||||
|
||||
## 控制器端口和命令
|
||||
|
||||
| 端口 | 主要用途 | 当前使用的命令 |
|
||||
| --- | --- | --- |
|
||||
| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` |
|
||||
| `19205` | 底盘控制 | `2000`、`2010`、`2022` |
|
||||
| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` |
|
||||
| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` |
|
||||
| `19210` | 开始/停止建图 | `6100`、`6101` |
|
||||
| `19301` | 机器人状态推送 | `9300`/`19300` 配置,`19301` 推送 |
|
||||
|
||||
所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005`
|
||||
抢权,负载为稳定的 `nick_name`,成功后才发送实际命令。普通命令集中走
|
||||
`sendControlledCommand_`;`emergencyStop` 为保证 `2000` 和导航取消之间不被
|
||||
插入其他命令,会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在
|
||||
gRPC 客户端另做一套租约逻辑。
|
||||
|
||||
## gRPC 接口概览
|
||||
|
||||
默认示例端点为 `127.0.0.1:50052`;远程部署时替换为 CMVR 服务所在主机,
|
||||
不是 SEER Robokit 原生 TCP 端口。
|
||||
|
||||
| gRPC 方法 | SEER Robokit 行为 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `getRuntimeState` | 推送缓存,缺失时查询 `1004/1007/1300` | 只读 |
|
||||
| `getNavigationStatus` | 跟踪任务查询 `1110`,无精确上下文时回退 `1020` | 只读;同步等待另用 `1101` 确认停车 |
|
||||
| `emergencyStop` | `2000`,再执行 `3003` 或 `3067` | 软件停止,不替代硬件急停 |
|
||||
| `clearFault` | 未实现 | 返回 `UnsupportedCommand` |
|
||||
| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿,仅双轮差速底盘 |
|
||||
| `navigateToStation` | `3051` | 站点路径导航;PGV 二次定位也使用此方法 |
|
||||
| `followPath` | `3066` | 仙工“指定路径导航”,与 `3051` 不同 |
|
||||
| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 |
|
||||
| `cancelNavigation` | `3003`,路径队列使用 `3067` | 取消当前跟踪任务 |
|
||||
| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度;停止时发送全零速度 |
|
||||
| `listMaps` / `listStations` | `1300` / `1301` | 只读 |
|
||||
| `switchMap` | `2022` | 会改变定位所用地图 |
|
||||
| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权,下载只读 |
|
||||
| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 |
|
||||
| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图,不暴露 `.smap` 原始格式 |
|
||||
|
||||
查询运行状态:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/getRuntimeState
|
||||
```
|
||||
|
||||
查询导航状态:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/getNavigationStatus
|
||||
```
|
||||
|
||||
列出地图和当前地图站点:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/listMaps
|
||||
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/listStations
|
||||
```
|
||||
|
||||
## 导航的同步语义
|
||||
|
||||
`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受
|
||||
命令后,适配器继续轮询精确任务状态,并结合 `1101` 状态确认底盘已经停车;
|
||||
到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用
|
||||
适配器默认值,当前为 10 分钟;`pollIntervalMs` 为 `0` 时当前使用 200 ms。
|
||||
|
||||
连续观察到障碍阻挡且底盘已经停止后,适配器会主动取消该导航;清理结果不明确
|
||||
时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、
|
||||
RPC cancel 和 deadline 到期也会进入安全取消及停车确认,因此函数返回时间可能
|
||||
晚于最初发现障碍或取消请求的时刻。
|
||||
|
||||
调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或
|
||||
deadline 到期时,适配器会进入安全取消/停车确认流程。显式设置
|
||||
`"asynchronous":true` 后不会等待任务终态:站点导航和指定路径导航在控制器
|
||||
接受后返回;自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。
|
||||
|
||||
`AgvMotionOptions` 中,SEER Robokit 的 `3051` 导航当前支持:
|
||||
|
||||
| gRPC 字段 | 控制器字段 | 单位 |
|
||||
| --- | --- | --- |
|
||||
| `maxSpeed` | `max_speed` | m/s |
|
||||
| `maxAngularSpeed` | `max_wspeed` | rad/s |
|
||||
| `maxAcceleration` | `max_acc` | m/s² |
|
||||
| `maxAngularAcceleration` | `max_wacc` | rad/s² |
|
||||
| `reachDistance` | `reach_dist` | m |
|
||||
| `reachAngle` | `reach_angle` | rad |
|
||||
|
||||
`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。
|
||||
`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只
|
||||
控制同步/异步等待、超时和轮询;在没有确认 `3066` 的速度字段前,不会猜测性地
|
||||
写入每个路径段。
|
||||
|
||||
## 固定路径导航的 PGV 二次定位
|
||||
|
||||
仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e)
|
||||
说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR
|
||||
中应调用 `navigateToStation`,不是 `followPath`。后者对应另一条
|
||||
`3066 / 指定路径导航` 协议,现有仙工资料和仓库历史都没有证明 `3066` 支持
|
||||
PGV 字段。
|
||||
|
||||
PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串,
|
||||
SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验,再转换为控制器
|
||||
要求的 JSON `bool`/`number`:
|
||||
|
||||
| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `use_pgv` | `bool` | 使用上视 PGV |
|
||||
| `use_down_pgv` | `bool` | 使用下视 PGV |
|
||||
| `pgv_adjust_dist` | `number` | 最大调整半径,必须为有限非负数;用于仙工第 3/4 种调整方式 |
|
||||
| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移;用于第 4 种方式 |
|
||||
| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移;用于第 4 种方式 |
|
||||
| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差;用于第 2 种方式 |
|
||||
|
||||
所有数字都必须是完整、有限的数字字符串;偏移量允许正负。适配器不臆造
|
||||
调整半径上限,也不假定上视和下视一定互斥,这些约束应由实际 PGV 安装、标定和
|
||||
当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值
|
||||
发给控制器;没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定,
|
||||
本接口只传递与所选方式匹配的调整参数。
|
||||
|
||||
一旦请求中出现任意 PGV 键,适配器只允许同时出现 `source_id`、`task_id` 和
|
||||
上述 PGV 字段;`operation`、`jack_height`、脚本名或未知扩展字段都会在状态
|
||||
查询和抢权前被拒绝,避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。
|
||||
没有 PGV 键的既有站点导航扩展语义保持不变。
|
||||
|
||||
上视 PGV 示例。该命令会让机器人导航到 `AP1`,只能在确认地图、站点、PGV
|
||||
标定、行驶区域和急停人员后执行:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"stationId":"AP1",
|
||||
"options":{
|
||||
"maxSpeed":0.15,
|
||||
"maxAcceleration":0.15,
|
||||
"asynchronous":false,
|
||||
"waitTimeoutMs":300000,
|
||||
"pollIntervalMs":200
|
||||
},
|
||||
"adapterParams":{"values":{
|
||||
"use_pgv":"true",
|
||||
"pgv_adjust_dist":"0.3",
|
||||
"pgv_adjust_cx":"-0.3",
|
||||
"pgv_adjust_cy":"0"
|
||||
}}
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/navigateToStation
|
||||
```
|
||||
|
||||
下视 PGV 使用同一接口,把 `use_down_pgv` 设为字符串 `"true"`;其他调整
|
||||
字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点,可在同一个
|
||||
map 中增加 `"source_id":"实际起点站点"`;默认起点为 `SELF_POSITION`。
|
||||
|
||||
仙工在线文档当前有两处拼写不一致:
|
||||
|
||||
- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`;适配器会拒绝它,正确字段是
|
||||
`pgv_adjust_dist`;
|
||||
- 表格写成 `pgv_ajdust_cy`,而示例和仓库旧版序列化代码使用
|
||||
`pgv_adjust_cy`。适配器兼容接收前者,但只向控制器输出规范字段
|
||||
`pgv_adjust_cy`;两个拼写同时出现会因歧义被拒绝。
|
||||
|
||||
C++ 调用同样复用通用扩展参数:
|
||||
|
||||
```cpp
|
||||
cmvr::device::AgvMotionOptions options;
|
||||
options.max_speed = 0.15;
|
||||
options.max_acceleration = 0.15;
|
||||
|
||||
cmvr::device::AgvAdapterParams adapter;
|
||||
adapter.values["use_pgv"] = "true";
|
||||
adapter.values["pgv_adjust_dist"] = "0.3";
|
||||
adapter.values["pgv_adjust_cx"] = "-0.3";
|
||||
adapter.values["pgv_adjust_cy"] = "0";
|
||||
|
||||
const auto result = agv.navigateToStation("AP1", options, adapter);
|
||||
```
|
||||
|
||||
## 其他导航和控制示例
|
||||
|
||||
自由导航使用地图绝对坐标,不是“相对当前位置移动多少米”。示例只展示请求
|
||||
结构,发送前必须读取当前位姿并确认目标在同一地图的安全区域:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"pose":{"x":1.0,"y":0.0,"theta":0.0},
|
||||
"options":{"maxSpeed":0.15,"maxAcceleration":0.15}
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/navigateToPose
|
||||
```
|
||||
|
||||
显式站点路径使用 `3066`:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{
|
||||
"header":{"deviceId":"src1100"},
|
||||
"path":[
|
||||
{"sourceStation":"LM1","targetStation":"LM2"},
|
||||
{"sourceStation":"LM2","targetStation":"AP1"}
|
||||
]
|
||||
}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/followPath
|
||||
```
|
||||
|
||||
暂停、继续和取消的请求体直接是 `CommandHeader.Request`,没有外层 `header`:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation
|
||||
```
|
||||
|
||||
差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航,并可能与已有任务
|
||||
冲突;只应在专门的速度控制测试流程中使用:
|
||||
|
||||
```bash
|
||||
grpcurl -plaintext \
|
||||
-d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \
|
||||
127.0.0.1:50052 \
|
||||
cmvr.api.AgvService/setVelocity
|
||||
|
||||
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
|
||||
127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl
|
||||
```
|
||||
|
||||
## 错误返回
|
||||
|
||||
控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`,并由
|
||||
gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC
|
||||
下,标准客户端通常不会交付响应体,因此跨客户端应以 transport status message
|
||||
为准,不要依赖反馈头仍然可见。例如:
|
||||
|
||||
```text
|
||||
SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose
|
||||
```
|
||||
|
||||
控制器仅返回“已接收”不等于导航完成;同步接口仍要等待精确任务终态和停车
|
||||
确认。若发送后连接中断且控制器是否执行已无法确定,错误会明确提示 outcome
|
||||
unknown,调用方不能自动重发运动命令,应先查询状态并取消或停止。
|
||||
|
||||
## 安全边界
|
||||
|
||||
- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口;不要把它当作
|
||||
多车调度接口,否则可能出现路径/速度不连续等危险行为。
|
||||
- `emergencyStop` 是控制器软件停止,不是功能安全急停;真实系统必须保留可达的
|
||||
硬件急停、安全激光、碰撞条和独立安全链。
|
||||
- 首次 PGV 测试应在低速、空载、隔离区域进行,并先核对二维码坐标系、传感器
|
||||
上/下视方向、调整半径和中心偏移的标定值。
|
||||
- PGV 同步成功目前能证明精确 `3051` 任务进入终态,并连续确认两次零速度;
|
||||
仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段,仍需实机
|
||||
验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。
|
||||
- 地图切换、地图上传和开始建图会改变控制器状态,也会先抢占控制权;不要和
|
||||
现场调度系统并行操作。
|
||||
- 本目录的假控制器测试验证软件协议、错误路径和并发逻辑,不代表真实 SEER Robokit、
|
||||
底盘、PGV、地图或安全链已经验收。
|
||||
|
||||
## 测试
|
||||
|
||||
```bash
|
||||
cmake --build build \
|
||||
--target seer_robokit_control_authority_test grpc_agv_service_test \
|
||||
-j2
|
||||
|
||||
ctest --test-dir build \
|
||||
-R '^(seer_robokit_control_authority_test|grpc_agv_service_test)$' \
|
||||
--output-on-failure
|
||||
```
|
||||
|
||||
`seer_robokit_control_authority_test` 使用本机回环 TCP 假控制器,需要允许本地
|
||||
bind/listen。受限沙箱若禁止创建 socket,只能证明编译通过,不能把未执行的
|
||||
fake-controller 场景报告为测试通过。测试过程不会连接真实 AGV。
|
||||
@ -1,5 +1,5 @@
|
||||
#ifndef CMVR_ES_SRC1100_AGV_H
|
||||
#define CMVR_ES_SRC1100_AGV_H
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
@ -18,14 +18,14 @@
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
class Src1100AgvTestPeer;
|
||||
class SeerRobokitAgvTestPeer;
|
||||
|
||||
class Src1100Agv final : public AbstractAGV {
|
||||
class SeerRobokitAgv final : public AbstractAGV {
|
||||
public:
|
||||
explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
|
||||
~Src1100Agv() override;
|
||||
explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg);
|
||||
~SeerRobokitAgv() override;
|
||||
|
||||
std::string typeName() const override { return "Src1100Agv"; }
|
||||
std::string typeName() const override { return "SeerRobokitAgv"; }
|
||||
|
||||
bool init() override;
|
||||
bool start() override;
|
||||
@ -46,7 +46,11 @@ public:
|
||||
const std::string& station_id,
|
||||
const AgvMotionOptions& options = {},
|
||||
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
||||
AgvResult followPath(const std::vector<AgvPathSegment>& path) override;
|
||||
AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path) override;
|
||||
AgvResult followPath(
|
||||
const std::vector<AgvPathSegment>& path,
|
||||
const AgvMotionOptions& options) override;
|
||||
AgvResult pauseNavigation() override;
|
||||
AgvResult resumeNavigation() override;
|
||||
AgvResult cancelNavigation() override;
|
||||
@ -67,7 +71,7 @@ public:
|
||||
AgvResult stopMapping() override;
|
||||
|
||||
private:
|
||||
friend class Src1100AgvTestPeer;
|
||||
friend class SeerRobokitAgvTestPeer;
|
||||
|
||||
struct Ports {
|
||||
int status{19204};
|
||||
@ -82,10 +86,46 @@ private:
|
||||
bool found{false};
|
||||
int state{0};
|
||||
int type{0};
|
||||
bool type_present{false};
|
||||
double progress{0.0};
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
struct NavigationSnapshot {
|
||||
int task_status{0};
|
||||
int task_type{0};
|
||||
bool task_status_present{false};
|
||||
bool task_type_present{false};
|
||||
bool blocked{false};
|
||||
bool blocked_present{false};
|
||||
int block_reason{-1};
|
||||
std::string block_reason_raw;
|
||||
bool velocity_present{false};
|
||||
double vx{0.0};
|
||||
double vy{0.0};
|
||||
double w{0.0};
|
||||
bool emergency{false};
|
||||
std::string target_id;
|
||||
std::string active_faults;
|
||||
std::string detail;
|
||||
};
|
||||
|
||||
enum class CommandTransmissionState {
|
||||
NotSent,
|
||||
PossiblySent,
|
||||
};
|
||||
|
||||
struct TrackedNavigationContext {
|
||||
std::string token;
|
||||
std::vector<std::string> task_ids;
|
||||
AgvTaskType type{AgvTaskType::None};
|
||||
std::string target_id;
|
||||
std::vector<std::string> target_ids;
|
||||
std::uint64_t navigation_generation{0};
|
||||
std::chrono::steady_clock::time_point accepted_at{};
|
||||
bool synchronous_wait{false};
|
||||
};
|
||||
|
||||
struct PoseTaskContext {
|
||||
std::string task_id;
|
||||
math::Pose2d target{};
|
||||
@ -99,6 +139,8 @@ private:
|
||||
|
||||
AgvResult connect_();
|
||||
AgvResult disconnect_();
|
||||
AgvResult emergencyStopTrackedNavigation_(
|
||||
const TrackedNavigationContext* expected_navigation);
|
||||
AgvResult connectSocket_(int& sock, int port);
|
||||
AgvResult ensureOtherSocket_();
|
||||
void closeSocket_(int& sock) const;
|
||||
@ -106,10 +148,35 @@ private:
|
||||
|
||||
AgvResult acquireControl_() const;
|
||||
AgvResult confirmPoseNavigationStarted_(
|
||||
const PoseTaskContext& context) const;
|
||||
const PoseTaskContext& context,
|
||||
bool accept_paused,
|
||||
const AgvMotionOptions& options) const;
|
||||
AgvResult waitForPoseNavigationTerminal_(
|
||||
const PoseTaskContext& pose_context,
|
||||
const TrackedNavigationContext& navigation_context,
|
||||
const AgvMotionOptions& options);
|
||||
AgvResult waitForTrackedNavigationTerminal_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options);
|
||||
AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const;
|
||||
AgvResult cancelTrackedNavigation_(
|
||||
const TrackedNavigationContext& context,
|
||||
std::uint64_t& accepted_generation);
|
||||
AgvResult waitForCanceledTaskToStop_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options,
|
||||
const std::string& reason);
|
||||
AgvResult failAndCancelTrackedNavigation_(
|
||||
const TrackedNavigationContext& context,
|
||||
const AgvMotionOptions& options,
|
||||
AgvErrorCode error_code,
|
||||
const std::string& reason);
|
||||
AgvResult queryPoseTaskStatus_(
|
||||
const std::string& task_id,
|
||||
PoseTaskStatus& status) const;
|
||||
AgvResult queryTaskStatuses_(
|
||||
const std::vector<std::string>& task_ids,
|
||||
std::vector<PoseTaskStatus>& statuses) const;
|
||||
bool poseTargetReached_(
|
||||
const PoseTaskContext& context,
|
||||
std::string& detail) const;
|
||||
@ -127,7 +194,16 @@ private:
|
||||
void advancePoseTaskControlAttempt_(
|
||||
std::uint64_t control_attempt_sequence) const;
|
||||
void clearPoseTask_(std::uint64_t navigation_generation) const;
|
||||
void clearPoseTaskIfTaskId_(const std::string& task_id) const;
|
||||
bool currentPoseTask_(PoseTaskContext& context) const;
|
||||
void rememberTrackedNavigation_(
|
||||
const TrackedNavigationContext& context) const;
|
||||
void advanceTrackedNavigationGeneration_(
|
||||
std::uint64_t navigation_generation) const;
|
||||
void clearTrackedNavigation_(std::uint64_t navigation_generation) const;
|
||||
void clearTrackedNavigationIfToken_(const std::string& token) const;
|
||||
bool currentTrackedNavigation_(
|
||||
TrackedNavigationContext& context) const;
|
||||
AgvResult sendControlledCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
@ -136,15 +212,22 @@ private:
|
||||
std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
|
||||
std::uint64_t* control_attempt_sequence = nullptr,
|
||||
PoseTaskContext* pose_context_to_publish = nullptr,
|
||||
bool reject_if_active_controller_fault = false) const;
|
||||
bool reject_if_active_controller_fault = false,
|
||||
TrackedNavigationContext* navigation_context_to_publish = nullptr,
|
||||
bool preserve_tracked_navigation = false,
|
||||
const std::string* expected_navigation_token = nullptr,
|
||||
const std::function<bool()>* cancellation_requested = nullptr,
|
||||
const TrackedNavigationContext* expected_active_navigation = nullptr) const;
|
||||
AgvResult sendCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response) const;
|
||||
Json::Value* response,
|
||||
CommandTransmissionState* transmission_state = nullptr) const;
|
||||
AgvResult sendCommandRaw_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload) const;
|
||||
std::string* response_payload,
|
||||
CommandTransmissionState* transmission_state = nullptr) const;
|
||||
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
|
||||
AgvResult configurePush_();
|
||||
void startPushThread_();
|
||||
@ -162,17 +245,17 @@ private:
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSrc1100MapArchive_(
|
||||
AgvResult parseSeerRobokitMapArchive_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSrc1100Map2D_(
|
||||
AgvResult parseSeerRobokitMap2D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
AgvResult parseSrc1100Map3D_(
|
||||
AgvResult parseSeerRobokitMap3D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
@ -198,7 +281,7 @@ private:
|
||||
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
|
||||
static AgvResult resultFromResponse_(const Json::Value& response);
|
||||
|
||||
config::Src1100AgvConfig config_;
|
||||
config::SeerRobokitAgvConfig config_;
|
||||
std::string ip_;
|
||||
std::string control_nick_name_;
|
||||
int recv_timeout_ms_{1000};
|
||||
@ -217,6 +300,8 @@ private:
|
||||
mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
|
||||
mutable std::mutex pose_task_mutex_;
|
||||
mutable PoseTaskContext pose_task_context_;
|
||||
mutable std::mutex tracked_navigation_mutex_;
|
||||
mutable TrackedNavigationContext tracked_navigation_context_;
|
||||
mutable int sock_status_{-1};
|
||||
mutable int sock_control_{-1};
|
||||
mutable int sock_navigation_{-1};
|
||||
@ -252,4 +337,4 @@ private:
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
#endif // CMVR_ES_SRC1100_AGV_H
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_AGV_H
|
||||
@ -0,0 +1,257 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
|
||||
namespace cmvr::device::seer_robokit::navigation {
|
||||
|
||||
constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500);
|
||||
constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
|
||||
constexpr int kPoseNavigationRequiredRunningSamples = 2;
|
||||
constexpr auto kDefaultNavigationWaitTimeout =
|
||||
std::chrono::milliseconds(600000);
|
||||
constexpr auto kDefaultNavigationPollInterval =
|
||||
std::chrono::milliseconds(200);
|
||||
constexpr auto kMaximumNavigationPollInterval =
|
||||
std::chrono::milliseconds(5000);
|
||||
constexpr auto kNavigationCancellationCheckInterval =
|
||||
std::chrono::milliseconds(50);
|
||||
constexpr auto kNavigationCancelPollInterval =
|
||||
std::chrono::milliseconds(100);
|
||||
constexpr auto kNavigationCancelConfirmationTimeout =
|
||||
std::chrono::milliseconds(3000);
|
||||
constexpr int kRequiredBlockedStopSamples = 2;
|
||||
constexpr int kRequiredCompletedStopSamples = 2;
|
||||
constexpr double kNavigationStopVelocityTolerance = 0.005;
|
||||
constexpr int kMinimumControllerFaultCaptureGraceMs = 250;
|
||||
constexpr int kMaximumControllerFaultCaptureGraceMs = 5000;
|
||||
constexpr int kDefaultControllerFaultPushIntervalMs = 1000;
|
||||
constexpr int kControllerFaultPushJitterMs = 100;
|
||||
constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000;
|
||||
constexpr int kControllerFaultStateMaxAgeIntervals = 5;
|
||||
constexpr double kDefaultPoseReachDistance = 0.05;
|
||||
constexpr double kDefaultPoseReachAngle = 0.10;
|
||||
constexpr double kTwoPi = 6.28318530717958647692;
|
||||
|
||||
static inline bool exactTaskStateIsActive(const int state)
|
||||
{
|
||||
return state >= 1 && state <= 3;
|
||||
}
|
||||
|
||||
static inline bool exactTaskStateIsKnownTerminal(const int state)
|
||||
{
|
||||
return state >= 4 && state <= 7;
|
||||
}
|
||||
|
||||
static inline bool globalTaskStateIsKnownTerminal(const int state)
|
||||
{
|
||||
return state == 0 || exactTaskStateIsKnownTerminal(state);
|
||||
}
|
||||
|
||||
static inline double angleDistance(const double lhs, const double rhs)
|
||||
{
|
||||
return std::abs(std::remainder(lhs - rhs, kTwoPi));
|
||||
}
|
||||
|
||||
static inline AgvResult withUnknownControllerOutcome(AgvResult result)
|
||||
{
|
||||
const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
|
||||
std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
|
||||
detail +=
|
||||
"; SEER Robokit controller outcome is unknown after the command attempt; "
|
||||
"the command may already have taken effect; do not issue another motion "
|
||||
"command automatically; query status and cancel or stop first";
|
||||
return AgvResult::failure(code, detail);
|
||||
}
|
||||
|
||||
static inline std::string makePoseTaskId(
|
||||
const std::string& device_id,
|
||||
const std::uint64_t task_sequence)
|
||||
{
|
||||
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
|
||||
return prefix + "_pose_" + std::to_string(timestamp)
|
||||
+ "_" + std::to_string(task_sequence);
|
||||
}
|
||||
|
||||
static inline std::string makeNavigationTaskId(
|
||||
const std::string& device_id,
|
||||
const char* kind,
|
||||
const std::uint64_t task_sequence)
|
||||
{
|
||||
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
|
||||
return prefix + "_" + kind + "_" + std::to_string(timestamp)
|
||||
+ "_" + std::to_string(task_sequence);
|
||||
}
|
||||
|
||||
static inline const char* blockReasonName(const int reason)
|
||||
{
|
||||
switch (reason) {
|
||||
case 0:
|
||||
return "ultrasonic";
|
||||
case 1:
|
||||
return "laser";
|
||||
case 2:
|
||||
return "fallingdown";
|
||||
case 3:
|
||||
return "collision";
|
||||
case 4:
|
||||
return "infrared";
|
||||
case 5:
|
||||
return "locked";
|
||||
default:
|
||||
return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
static inline std::string invalidMotionOption(const AgvMotionOptions& options)
|
||||
{
|
||||
const auto non_negative_error = [](const double value, const char* field) {
|
||||
if (!std::isfinite(value)) {
|
||||
return std::string(field) + " must be finite";
|
||||
}
|
||||
if (value < 0.0) {
|
||||
return std::string(field) + " must be non-negative";
|
||||
}
|
||||
return std::string{};
|
||||
};
|
||||
|
||||
if (auto error = non_negative_error(options.max_speed, "max_speed");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_angular_speed,
|
||||
"max_angular_speed");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_acceleration,
|
||||
"max_acceleration");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.max_angular_acceleration,
|
||||
"max_angular_acceleration");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(
|
||||
options.reach_distance,
|
||||
"reach_distance");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(options.reach_angle, "reach_angle");
|
||||
!error.empty()) return error;
|
||||
if (auto error = non_negative_error(options.speed_ratio, "speed_ratio");
|
||||
!error.empty()) return error;
|
||||
if (options.wait_timeout_ms < 0) {
|
||||
return "wait_timeout_ms must be non-negative";
|
||||
}
|
||||
if (options.poll_interval_ms < 0) {
|
||||
return "poll_interval_ms must be non-negative";
|
||||
}
|
||||
if (options.poll_interval_ms
|
||||
> kMaximumNavigationPollInterval.count()) {
|
||||
return "poll_interval_ms must not exceed "
|
||||
+ std::to_string(kMaximumNavigationPollInterval.count());
|
||||
}
|
||||
if (options.wait_timeout_ms > 0
|
||||
&& options.poll_interval_ms > options.wait_timeout_ms) {
|
||||
return "poll_interval_ms must not exceed wait_timeout_ms";
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
static inline std::chrono::milliseconds navigationWaitTimeout(
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
return options.wait_timeout_ms > 0
|
||||
? std::chrono::milliseconds(options.wait_timeout_ms)
|
||||
: kDefaultNavigationWaitTimeout;
|
||||
}
|
||||
|
||||
static inline std::chrono::milliseconds navigationPollInterval(
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
if (options.poll_interval_ms <= 0) {
|
||||
return kDefaultNavigationPollInterval;
|
||||
}
|
||||
return std::chrono::milliseconds(
|
||||
std::max(options.poll_interval_ms, 20));
|
||||
}
|
||||
|
||||
static inline bool navigationCancellationRequested(const AgvMotionOptions& options)
|
||||
{
|
||||
return options.cancellation_requested
|
||||
&& options.cancellation_requested();
|
||||
}
|
||||
|
||||
static inline void sleepForNavigationPoll(
|
||||
const std::chrono::milliseconds poll_interval,
|
||||
const std::chrono::steady_clock::time_point overall_deadline,
|
||||
const AgvMotionOptions& options)
|
||||
{
|
||||
const auto poll_deadline = std::min(
|
||||
overall_deadline,
|
||||
std::chrono::steady_clock::now() + poll_interval);
|
||||
while (std::chrono::steady_clock::now() < poll_deadline
|
||||
&& !navigationCancellationRequested(options)) {
|
||||
const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
poll_deadline - std::chrono::steady_clock::now());
|
||||
if (remaining <= std::chrono::milliseconds::zero()) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::min(
|
||||
kNavigationCancellationCheckInterval,
|
||||
remaining));
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Snapshot>
|
||||
static inline bool navigationStopped(const Snapshot& snapshot)
|
||||
{
|
||||
return snapshot.velocity_present
|
||||
&& std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance
|
||||
&& std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance
|
||||
&& std::abs(snapshot.w) <= kNavigationStopVelocityTolerance;
|
||||
}
|
||||
|
||||
static inline AgvResult reconciledNavigationResult(
|
||||
const AgvResult& command_result,
|
||||
AgvResult terminal_result)
|
||||
{
|
||||
if (command_result.ok()) {
|
||||
return terminal_result;
|
||||
}
|
||||
if (terminal_result.ok()) {
|
||||
terminal_result.message =
|
||||
"SEER Robokit navigation completed after an indeterminate command "
|
||||
"acknowledgment; initial_detail=" + command_result.message;
|
||||
return terminal_result;
|
||||
}
|
||||
terminal_result.message =
|
||||
"SEER Robokit navigation command acknowledgment was indeterminate: "
|
||||
+ command_result.message + "; status reconciliation: "
|
||||
+ terminal_result.message;
|
||||
return terminal_result;
|
||||
}
|
||||
|
||||
static inline bool parseFiniteDouble(const std::string& value, double& parsed)
|
||||
{
|
||||
std::size_t consumed = 0;
|
||||
try {
|
||||
parsed = std::stod(value, &consumed);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
return consumed == value.size() && std::isfinite(parsed);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::navigation
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
|
||||
@ -0,0 +1,141 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <json/json.h>
|
||||
|
||||
#include "devices/agv/abstract_agv.h"
|
||||
#include "seer_robokit_navigation_utils.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
namespace cmvr::device::seer_robokit::pgv {
|
||||
|
||||
constexpr char kUsePgv[] = "use_pgv";
|
||||
constexpr char kPgvAdjustDist[] = "pgv_adjust_dist";
|
||||
constexpr char kPgvAdjustCx[] = "pgv_adjust_cx";
|
||||
constexpr char kPgvAdjustCy[] = "pgv_adjust_cy";
|
||||
constexpr char kPgvXAdjust[] = "pgv_x_adjust";
|
||||
constexpr char kUseDownPgv[] = "use_down_pgv";
|
||||
|
||||
// These spellings currently appear in the vendor document, but conflict with
|
||||
// its own field table/example and the repository's older working serializer.
|
||||
constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist";
|
||||
constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy";
|
||||
|
||||
static inline bool isPgvAdjustmentKey(const std::string& key)
|
||||
{
|
||||
return key == kUsePgv
|
||||
|| key == kPgvAdjustDist
|
||||
|| key == kPgvAdjustCx
|
||||
|| key == kPgvAdjustCy
|
||||
|| key == kPgvXAdjust
|
||||
|| key == kUseDownPgv
|
||||
|| key == kMalformedAdjustDist
|
||||
|| key == kAdjustCyDocumentAlias;
|
||||
}
|
||||
|
||||
static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params)
|
||||
{
|
||||
for (const auto& [key, value] : params.values) {
|
||||
(void)value;
|
||||
if (isPgvAdjustmentKey(key)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Parse the string-valued generic adapter parameters into the native JSON
|
||||
* types required by SEER Robokit API 3051. Returns an error string without
|
||||
* modifying controller state; an empty string means success.
|
||||
*/
|
||||
static inline std::string applyPgvAdjustmentParams(
|
||||
Json::Value& payload,
|
||||
const AgvAdapterParams& params)
|
||||
{
|
||||
if (params.getString(kMalformedAdjustDist)) {
|
||||
return std::string(kMalformedAdjustDist)
|
||||
+ " is a vendor-document typo; use " + kPgvAdjustDist;
|
||||
}
|
||||
if (hasPgvAdjustmentParams(params)) {
|
||||
for (const auto& [key, value] : params.values) {
|
||||
(void)value;
|
||||
if (!isPgvAdjustmentKey(key)
|
||||
&& key != "source_id"
|
||||
&& key != "task_id") {
|
||||
return "PGV adjustment must not be combined with adapter "
|
||||
"field " + key;
|
||||
}
|
||||
}
|
||||
}
|
||||
const auto adjust_cy = params.getString(kPgvAdjustCy);
|
||||
const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias);
|
||||
if (adjust_cy && adjust_cy_alias) {
|
||||
return std::string(kPgvAdjustCy) + " and its vendor-document alias "
|
||||
+ kAdjustCyDocumentAlias + " must not both be set";
|
||||
}
|
||||
|
||||
const auto apply_bool = [&payload, ¶ms](const char* key) {
|
||||
if (!params.getString(key)) {
|
||||
return std::string{};
|
||||
}
|
||||
const auto parsed = params.getBool(key);
|
||||
if (!parsed) {
|
||||
return std::string(key)
|
||||
+ " must be a boolean string such as true or false";
|
||||
}
|
||||
detail::jsonMember(payload, key) = *parsed;
|
||||
return std::string{};
|
||||
};
|
||||
if (auto error = apply_bool(kUsePgv); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_bool(kUseDownPgv); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
|
||||
const auto apply_number = [&payload, ¶ms](
|
||||
const char* key,
|
||||
const bool non_negative) {
|
||||
const auto raw = params.getString(key);
|
||||
if (!raw) {
|
||||
return std::string{};
|
||||
}
|
||||
double parsed = 0.0;
|
||||
if (!navigation::parseFiniteDouble(*raw, parsed)) {
|
||||
return std::string(key) + " must be a complete finite number";
|
||||
}
|
||||
if (non_negative && parsed < 0.0) {
|
||||
return std::string(key) + " must be non-negative";
|
||||
}
|
||||
detail::jsonMember(payload, key) = parsed;
|
||||
return std::string{};
|
||||
};
|
||||
if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (adjust_cy_alias) {
|
||||
double parsed = 0.0;
|
||||
if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) {
|
||||
return std::string(kAdjustCyDocumentAlias)
|
||||
+ " must be a complete finite number";
|
||||
}
|
||||
detail::jsonMember(payload, kPgvAdjustCy) = parsed;
|
||||
} else if (auto error = apply_number(kPgvAdjustCy, false);
|
||||
!error.empty()) {
|
||||
return error;
|
||||
}
|
||||
if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) {
|
||||
return error;
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::pgv
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
|
||||
@ -0,0 +1,38 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace cmvr::device::seer_robokit::protocol {
|
||||
|
||||
constexpr std::uint16_t kRobotStatusLoc = 1004;
|
||||
constexpr std::uint16_t kRobotStatusBattery = 1007;
|
||||
constexpr std::uint16_t kRobotStatusAll2 = 1101;
|
||||
constexpr std::uint16_t kRobotStatusTask = 1020;
|
||||
constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
|
||||
constexpr std::uint16_t kRobotStatusMap = 1300;
|
||||
constexpr std::uint16_t kRobotStatusStation = 1301;
|
||||
constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
|
||||
constexpr std::uint16_t kRobotStatusDownloadFile = 1800;
|
||||
constexpr std::uint16_t kRobotControlStop = 2000;
|
||||
constexpr std::uint16_t kRobotControlMotion = 2010;
|
||||
constexpr std::uint16_t kRobotControlLoadMap = 2022;
|
||||
constexpr std::uint16_t kRobotTaskPause = 3001;
|
||||
constexpr std::uint16_t kRobotTaskResume = 3002;
|
||||
constexpr std::uint16_t kRobotTaskCancel = 3003;
|
||||
constexpr std::uint16_t kRobotTaskGoTarget = 3051;
|
||||
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
|
||||
constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
|
||||
constexpr std::uint16_t kRobotConfigLock = 4005;
|
||||
constexpr std::uint16_t kRobotConfigUploadMap = 4010;
|
||||
constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
|
||||
constexpr std::uint16_t kRobotOtherStartMapping = 6100;
|
||||
constexpr std::uint16_t kRobotOtherStopMapping = 6101;
|
||||
constexpr std::uint16_t kRobotPushConfigReq = 9300;
|
||||
constexpr std::uint16_t kRobotPushConfigRes = 19300;
|
||||
constexpr std::uint16_t kRobotPush = 19301;
|
||||
constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U;
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::protocol
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
|
||||
@ -0,0 +1,92 @@
|
||||
#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
#define CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include <json/json.h>
|
||||
|
||||
namespace cmvr::device::seer_robokit::detail {
|
||||
|
||||
static inline std::string systemError()
|
||||
{
|
||||
return std::strerror(errno);
|
||||
}
|
||||
|
||||
static inline Json::Value& jsonMember(
|
||||
Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return *value.demand(key, key + std::strlen(key));
|
||||
}
|
||||
|
||||
static inline Json::Value& jsonMember(
|
||||
Json::Value& value,
|
||||
const std::string& key)
|
||||
{
|
||||
return *value.demand(key.data(), key.data() + key.size());
|
||||
}
|
||||
|
||||
static inline const Json::Value* jsonFind(
|
||||
const Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return value.find(key, key + std::strlen(key));
|
||||
}
|
||||
|
||||
static inline Json::Value jsonGet(
|
||||
const Json::Value& value,
|
||||
const char* key,
|
||||
const Json::Value& fallback)
|
||||
{
|
||||
const auto* found = jsonFind(value, key);
|
||||
return found ? *found : fallback;
|
||||
}
|
||||
|
||||
static inline double nowSeconds()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now().time_since_epoch();
|
||||
return std::chrono::duration<double>(now).count();
|
||||
}
|
||||
|
||||
static inline bool jsonHas(
|
||||
const Json::Value& value,
|
||||
const char* key)
|
||||
{
|
||||
return jsonFind(value, key) != nullptr;
|
||||
}
|
||||
|
||||
static inline bool hasNumericControllerRetCode(
|
||||
const Json::Value& response)
|
||||
{
|
||||
const auto* ret_code = jsonFind(response, "ret_code");
|
||||
return ret_code
|
||||
&& (ret_code->isInt()
|
||||
|| ret_code->isUInt()
|
||||
|| ret_code->isInt64()
|
||||
|| ret_code->isUInt64());
|
||||
}
|
||||
|
||||
static inline std::string jsonValueToString(const Json::Value& value)
|
||||
{
|
||||
if (value.isString()) return value.asString();
|
||||
if (value.isBool()) return value.asBool() ? "true" : "false";
|
||||
if (value.isInt64() || value.isInt()) {
|
||||
return std::to_string(value.asInt64());
|
||||
}
|
||||
if (value.isUInt64() || value.isUInt()) {
|
||||
return std::to_string(value.asUInt64());
|
||||
}
|
||||
if (value.isDouble()) return std::to_string(value.asDouble());
|
||||
if (value.isNull()) return {};
|
||||
|
||||
Json::StreamWriterBuilder builder;
|
||||
builder["indentation"] = "";
|
||||
return Json::writeString(builder, value);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device::seer_robokit::detail
|
||||
|
||||
#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H
|
||||
175
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp
Normal file
175
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp
Normal file
@ -0,0 +1,175 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <cstddef>
|
||||
#include <mutex>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int kDefaultMapUpdateIntervalMs = 1000;
|
||||
constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
|
||||
|
||||
} // namespace
|
||||
|
||||
SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg)
|
||||
: config_(cfg),
|
||||
ip_(cfg.ip()),
|
||||
control_nick_name_(
|
||||
cfg.control_nick_name().empty()
|
||||
? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id())
|
||||
: cfg.control_nick_name()),
|
||||
recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000),
|
||||
state_push_enabled_(cfg.enable_state_push()),
|
||||
map_update_enabled_(cfg.enable_map_update()),
|
||||
map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs),
|
||||
map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize)
|
||||
{
|
||||
id_ = cfg.id();
|
||||
if (cfg.port_status() > 0) ports_.status = cfg.port_status();
|
||||
if (cfg.port_control() > 0) ports_.control = cfg.port_control();
|
||||
if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav();
|
||||
if (cfg.port_config() > 0) ports_.config = cfg.port_config();
|
||||
if (cfg.port_other() > 0) ports_.other = cfg.port_other();
|
||||
if (cfg.port_push() > 0) ports_.push = cfg.port_push();
|
||||
|
||||
const auto result = connect_();
|
||||
if (!result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Auto connect failed"
|
||||
<< ", id=" << id_
|
||||
<< ", ip=" << ip_
|
||||
<< ", error=" << result.message;
|
||||
}
|
||||
}
|
||||
|
||||
SeerRobokitAgv::~SeerRobokitAgv()
|
||||
{
|
||||
(void)disconnect_();
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::init()
|
||||
{
|
||||
return !id_.empty() && !ip_.empty();
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::start()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::stop()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::update()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::connect_()
|
||||
{
|
||||
const auto lifecycle_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
clearPoseTask_(lifecycle_generation);
|
||||
clearTrackedNavigation_(lifecycle_generation);
|
||||
stopPushThread_();
|
||||
stopMapUpdateThread_();
|
||||
|
||||
{
|
||||
// Status requests may wait for a controller receive timeout without
|
||||
// holding mutex_. Serialize lifecycle changes with that channel before
|
||||
// replacing or closing its descriptor.
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
|
||||
if (ip_.empty()) {
|
||||
return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit AGV ip is empty");
|
||||
}
|
||||
|
||||
const auto close_all = [this]() {
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
};
|
||||
|
||||
if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) {
|
||||
close_all();
|
||||
return result;
|
||||
}
|
||||
|
||||
if (state_push_enabled_) {
|
||||
const auto result = connectSocket_(sock_push_, ports_.push);
|
||||
if (!result.ok()) {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Connect push port failed"
|
||||
<< ", id=" << id_
|
||||
<< ", port=" << ports_.push
|
||||
<< ", error=" << result.message;
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
}
|
||||
last_error_.clear();
|
||||
}
|
||||
|
||||
if (state_push_enabled_ && sock_push_ >= 0) {
|
||||
const auto result = configurePush_();
|
||||
if (result.ok()) {
|
||||
startPushThread_();
|
||||
} else {
|
||||
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Configure push failed"
|
||||
<< ", id=" << id_
|
||||
<< ", error=" << result.message;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
}
|
||||
if (map_update_enabled_) {
|
||||
startMapUpdateThread_();
|
||||
}
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::disconnect_()
|
||||
{
|
||||
const auto lifecycle_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
clearPoseTask_(lifecycle_generation);
|
||||
clearTrackedNavigation_(lifecycle_generation);
|
||||
stopMapUpdateThread_();
|
||||
stopPushThread_();
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
closeSocket_(sock_navigation_);
|
||||
closeSocket_(sock_config_);
|
||||
closeSocket_(sock_other_);
|
||||
closeSocket_(sock_push_);
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
378
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
Normal file
378
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
Normal file
@ -0,0 +1,378 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_navigation_utils.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::navigation;
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
AgvResult SeerRobokitAgv::acquireControl_() const
|
||||
{
|
||||
Json::Value payload(Json::objectValue);
|
||||
jsonMember(payload, "nick_name") = control_nick_name_;
|
||||
|
||||
Json::Value response;
|
||||
auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendControlledCommand_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
std::uint64_t* accepted_navigation_generation,
|
||||
std::uint64_t* controller_fault_sequence_at_attempt,
|
||||
std::uint64_t* control_attempt_sequence,
|
||||
PoseTaskContext* pose_context_to_publish,
|
||||
const bool reject_if_active_controller_fault,
|
||||
TrackedNavigationContext* navigation_context_to_publish,
|
||||
const bool preserve_tracked_navigation,
|
||||
const std::string* expected_navigation_token,
|
||||
const std::function<bool()>* cancellation_requested,
|
||||
const TrackedNavigationContext* expected_active_navigation) const
|
||||
{
|
||||
const auto canceled_before_send = [cancellation_requested]() {
|
||||
return cancellation_requested
|
||||
&& *cancellation_requested
|
||||
&& (*cancellation_requested)();
|
||||
};
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation before control authority was acquired");
|
||||
}
|
||||
|
||||
const auto expected_context_is_current =
|
||||
[this, expected_active_navigation]() {
|
||||
if (!expected_active_navigation) {
|
||||
return true;
|
||||
}
|
||||
TrackedNavigationContext active_context;
|
||||
return currentTrackedNavigation_(active_context)
|
||||
&& active_context.token
|
||||
== expected_active_navigation->token
|
||||
&& active_context.navigation_generation
|
||||
== expected_active_navigation->navigation_generation
|
||||
&& active_context.type
|
||||
== expected_active_navigation->type
|
||||
&& navigation_generation_.load(std::memory_order_relaxed)
|
||||
== expected_active_navigation->navigation_generation;
|
||||
};
|
||||
|
||||
// Conditional cancel ownership checks are deliberately performed without
|
||||
// the control sequencing mutex. A slow 1110/1101 response must never
|
||||
// prevent emergencyStop() from acquiring authority and sending 2000.
|
||||
// The exact local token/generation/type is revalidated under the control
|
||||
// lock both before and after authority acquisition below.
|
||||
if (expected_active_navigation) {
|
||||
if (!expected_context_is_current()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not start conditional navigation cancel "
|
||||
"preflight because the tracked task was already replaced or "
|
||||
"ended");
|
||||
}
|
||||
|
||||
std::vector<PoseTaskStatus> statuses;
|
||||
const auto exact_result = queryTaskStatuses_(
|
||||
expected_active_navigation->task_ids,
|
||||
statuses);
|
||||
if (!exact_result.ok()) {
|
||||
return AgvResult::failure(
|
||||
exact_result.code,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because exact task ownership preflight failed: "
|
||||
+ exact_result.message);
|
||||
}
|
||||
const bool all_exact_tasks_terminal = !statuses.empty()
|
||||
&& std::all_of(
|
||||
statuses.begin(),
|
||||
statuses.end(),
|
||||
[](const PoseTaskStatus& status) {
|
||||
return status.found
|
||||
&& exactTaskStateIsKnownTerminal(status.state);
|
||||
});
|
||||
if (all_exact_tasks_terminal) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
const bool exact_task_still_active = std::any_of(
|
||||
statuses.begin(),
|
||||
statuses.end(),
|
||||
[](const PoseTaskStatus& status) {
|
||||
return status.found
|
||||
&& exactTaskStateIsActive(status.state);
|
||||
});
|
||||
|
||||
NavigationSnapshot snapshot;
|
||||
const auto snapshot_result = queryNavigationSnapshot_(snapshot);
|
||||
if (!snapshot_result.ok()) {
|
||||
return AgvResult::failure(
|
||||
snapshot_result.code,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because 1101 ownership preflight was unavailable: "
|
||||
+ snapshot_result.message);
|
||||
}
|
||||
const int expected_type = expected_active_navigation->type
|
||||
== AgvTaskType::NavigateToPose
|
||||
? 1
|
||||
: (expected_active_navigation->type
|
||||
== AgvTaskType::NavigateToStation
|
||||
? 2
|
||||
: 3);
|
||||
const bool global_active =
|
||||
exactTaskStateIsActive(snapshot.task_status);
|
||||
const bool target_conflicts = global_active
|
||||
&& !snapshot.target_id.empty()
|
||||
&& !expected_active_navigation->target_ids.empty()
|
||||
&& std::find(
|
||||
expected_active_navigation->target_ids.begin(),
|
||||
expected_active_navigation->target_ids.end(),
|
||||
snapshot.target_id)
|
||||
== expected_active_navigation->target_ids.end();
|
||||
if (global_active
|
||||
&& (snapshot.task_type != expected_type
|
||||
|| target_conflicts)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because 1101 reports another active task: "
|
||||
+ snapshot.detail);
|
||||
}
|
||||
|
||||
if (!exact_task_still_active) {
|
||||
const bool clearing_path_queue =
|
||||
expected_active_navigation->type
|
||||
== AgvTaskType::FollowPath;
|
||||
const bool terminal_target_matches =
|
||||
expected_active_navigation->type
|
||||
!= AgvTaskType::NavigateToStation
|
||||
|| (!snapshot.target_id.empty()
|
||||
&& snapshot.target_id
|
||||
== expected_active_navigation->target_id);
|
||||
if (globalTaskStateIsKnownTerminal(snapshot.task_status)
|
||||
&& snapshot.task_status != 0
|
||||
&& !clearing_path_queue
|
||||
&& snapshot.task_type == expected_type
|
||||
&& terminal_target_matches) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Keep the permission acquisition and the following write ordered with
|
||||
// respect to other control RPCs in this process. Channel I/O serialization
|
||||
// is separate, so this must remain a distinct lock.
|
||||
std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
|
||||
if (expected_navigation_token) {
|
||||
TrackedNavigationContext active_context;
|
||||
const bool has_active_context =
|
||||
currentTrackedNavigation_(active_context);
|
||||
const bool expected_context_matches = expected_active_navigation
|
||||
? (has_active_context
|
||||
&& active_context.token
|
||||
== expected_active_navigation->token
|
||||
&& active_context.navigation_generation
|
||||
== expected_active_navigation->navigation_generation
|
||||
&& active_context.type
|
||||
== expected_active_navigation->type
|
||||
&& navigation_generation_.load(std::memory_order_relaxed)
|
||||
== expected_active_navigation->navigation_generation)
|
||||
: (has_active_context
|
||||
&& active_context.token == *expected_navigation_token);
|
||||
if (!expected_context_matches) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel "
|
||||
"because the tracked task was already replaced or ended; "
|
||||
"expected_token=" + *expected_navigation_token
|
||||
+ (active_context.token.empty()
|
||||
? std::string(", active_token=<none>")
|
||||
: ", active_token=" + active_context.token));
|
||||
}
|
||||
}
|
||||
const auto attempt_sequence =
|
||||
control_attempt_sequence_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed) + 1;
|
||||
if (control_attempt_sequence) {
|
||||
*control_attempt_sequence = attempt_sequence;
|
||||
}
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->control_attempt_sequence_at_start =
|
||||
attempt_sequence;
|
||||
}
|
||||
|
||||
const auto authority = acquireControl_();
|
||||
if (!authority.ok()) {
|
||||
const std::string detail = authority.message.empty() ? "unknown error" : authority.message;
|
||||
return AgvResult::failure(
|
||||
authority.code,
|
||||
"SEER Robokit acquire control authority failed: " + detail);
|
||||
}
|
||||
if (expected_active_navigation && !expected_context_is_current()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit did not send the conditional navigation cancel because "
|
||||
"the tracked token, generation, or type changed while control "
|
||||
"authority was being acquired");
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation while control authority was being acquired");
|
||||
}
|
||||
std::string controller_fault_gate_error;
|
||||
if (controller_fault_sequence_at_attempt
|
||||
|| pose_context_to_publish
|
||||
|| reject_if_active_controller_fault) {
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
if (controller_fault_sequence_at_attempt) {
|
||||
*controller_fault_sequence_at_attempt =
|
||||
controller_fault_sequence_;
|
||||
}
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->controller_fault_sequence_at_start =
|
||||
controller_fault_sequence_;
|
||||
pose_context_to_publish
|
||||
->controller_fault_channel_epoch_at_start =
|
||||
controller_fault_channel_epoch_.load(
|
||||
std::memory_order_relaxed);
|
||||
}
|
||||
if (reject_if_active_controller_fault) {
|
||||
if (!state_push_enabled_) {
|
||||
controller_fault_gate_error =
|
||||
"controller fault state is unavailable because state push "
|
||||
"is disabled";
|
||||
} else if (!active_controller_fault_detail_.empty()) {
|
||||
controller_fault_gate_error =
|
||||
"the controller reported a fault or invalid fault state: "
|
||||
+ active_controller_fault_detail_;
|
||||
} else if (!controller_fault_state_observed_) {
|
||||
controller_fault_gate_error =
|
||||
"no state push containing fatals/errors has been observed";
|
||||
} else {
|
||||
const auto fault_state_age =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now()
|
||||
- controller_fault_state_observed_at_)
|
||||
.count();
|
||||
if (fault_state_age > controllerFaultStateMaxAgeMs_()) {
|
||||
controller_fault_gate_error =
|
||||
"the most recent fatals/errors state push is stale "
|
||||
"(age_ms=" + std::to_string(fault_state_age)
|
||||
+ ", max_age_ms="
|
||||
+ std::to_string(controllerFaultStateMaxAgeMs_())
|
||||
+ ")";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!controller_fault_gate_error.empty()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::Fault,
|
||||
"SEER Robokit free-navigation command was not sent because "
|
||||
+ controller_fault_gate_error);
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation before the controller command write");
|
||||
}
|
||||
if (canceled_before_send()) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskCanceled,
|
||||
"SEER Robokit command was not sent because the caller canceled the "
|
||||
"operation immediately before the controller command write");
|
||||
}
|
||||
const auto publish_navigation_generation =
|
||||
[this,
|
||||
accepted_navigation_generation,
|
||||
pose_context_to_publish,
|
||||
navigation_context_to_publish,
|
||||
preserve_tracked_navigation]() {
|
||||
const auto generation =
|
||||
navigation_generation_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed) + 1;
|
||||
*accepted_navigation_generation = generation;
|
||||
if (pose_context_to_publish) {
|
||||
pose_context_to_publish->navigation_generation = generation;
|
||||
rememberPoseTask_(*pose_context_to_publish);
|
||||
}
|
||||
if (navigation_context_to_publish) {
|
||||
navigation_context_to_publish->navigation_generation =
|
||||
generation;
|
||||
navigation_context_to_publish->accepted_at =
|
||||
std::chrono::steady_clock::now();
|
||||
rememberTrackedNavigation_(
|
||||
*navigation_context_to_publish);
|
||||
} else if (preserve_tracked_navigation) {
|
||||
advanceTrackedNavigationGeneration_(generation);
|
||||
} else {
|
||||
clearTrackedNavigation_(generation);
|
||||
}
|
||||
};
|
||||
CommandTransmissionState transmission_state =
|
||||
CommandTransmissionState::NotSent;
|
||||
auto result = sendCommand_(
|
||||
sock,
|
||||
command,
|
||||
payload,
|
||||
response,
|
||||
&transmission_state);
|
||||
if (!result.ok()) {
|
||||
if (accepted_navigation_generation
|
||||
&& transmission_state
|
||||
== CommandTransmissionState::PossiblySent) {
|
||||
// Once the control write has been attempted, a timeout, disconnect,
|
||||
// wrong response opcode, or malformed JSON cannot prove rejection:
|
||||
// the controller may already have executed the command.
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
if (!accepted_navigation_generation) {
|
||||
return result;
|
||||
}
|
||||
if (!response) {
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit cannot confirm navigation command without a response"));
|
||||
}
|
||||
if (!hasNumericControllerRetCode(*response)) {
|
||||
publish_navigation_generation();
|
||||
return withUnknownControllerOutcome(resultFromResponse_(*response));
|
||||
}
|
||||
result = resultFromResponse_(*response);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
|
||||
// Advance only after the controller accepted the command, and do it before
|
||||
// releasing control_sequence_mutex_. This prevents a failed cancel/pause or
|
||||
// failed authority acquisition from falsely reporting a pose task canceled,
|
||||
// while preserving the controller's actual command order under concurrency.
|
||||
publish_navigation_generation();
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
1016
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp
Normal file
1016
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1378
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
Normal file
1378
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
725
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
Normal file
725
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
Normal file
@ -0,0 +1,725 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <sys/socket.h>
|
||||
#include <thread>
|
||||
|
||||
#include <google/protobuf/repeated_ptr_field.h>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
namespace {
|
||||
|
||||
bool hasFaultArray(const Json::Value& value, const char* key)
|
||||
{
|
||||
const auto* found = jsonFind(value, key);
|
||||
return found && found->isArray() && !found->empty();
|
||||
}
|
||||
|
||||
void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField<std::string>& strings)
|
||||
{
|
||||
if (strings.empty()) {
|
||||
return;
|
||||
}
|
||||
Json::Value array(Json::arrayValue);
|
||||
for (const auto& item : strings) {
|
||||
array.append(item);
|
||||
}
|
||||
jsonMember(value, key) = array;
|
||||
}
|
||||
|
||||
AgvMode modeFromTaskState(const int state)
|
||||
{
|
||||
switch (state) {
|
||||
case 2:
|
||||
return AgvMode::Auto;
|
||||
case 3:
|
||||
return AgvMode::Paused;
|
||||
case 5:
|
||||
return AgvMode::Fault;
|
||||
case 6:
|
||||
return AgvMode::Stopped;
|
||||
default:
|
||||
return AgvMode::Idle;
|
||||
}
|
||||
}
|
||||
|
||||
AgvTaskState toTaskState(const int value)
|
||||
{
|
||||
switch (value) {
|
||||
case 1:
|
||||
return AgvTaskState::Waiting;
|
||||
case 2:
|
||||
return AgvTaskState::Running;
|
||||
case 3:
|
||||
return AgvTaskState::Paused;
|
||||
case 4:
|
||||
return AgvTaskState::Completed;
|
||||
case 5:
|
||||
case 7:
|
||||
return AgvTaskState::Failed;
|
||||
case 6:
|
||||
return AgvTaskState::Canceled;
|
||||
case 0:
|
||||
default:
|
||||
return AgvTaskState::None;
|
||||
}
|
||||
}
|
||||
|
||||
AgvTaskType toTaskType(const int value)
|
||||
{
|
||||
switch (value) {
|
||||
case 1:
|
||||
return AgvTaskType::NavigateToPose;
|
||||
case 2:
|
||||
return AgvTaskType::NavigateToStation;
|
||||
case 3:
|
||||
return AgvTaskType::FollowPath;
|
||||
case 100:
|
||||
return AgvTaskType::Custom;
|
||||
default:
|
||||
return AgvTaskType::None;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
AgvRuntimeState SeerRobokitAgv::runtimeState() const
|
||||
{
|
||||
AgvRuntimeState cached_state;
|
||||
bool has_cached_state = false;
|
||||
if (state_push_enabled_) {
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
if (cached_runtime_state_valid_) {
|
||||
cached_state = cached_runtime_state_;
|
||||
has_cached_state = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (has_cached_state) {
|
||||
std::string adapter_error;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
cached_state.connected = connected_();
|
||||
adapter_error = last_error_;
|
||||
}
|
||||
if (!adapter_error.empty()) {
|
||||
if (cached_state.last_error.empty()) {
|
||||
cached_state.last_error = adapter_error;
|
||||
} else if (cached_state.last_error != adapter_error) {
|
||||
cached_state.last_error += "; adapter_error=" + adapter_error;
|
||||
}
|
||||
}
|
||||
if (!cached_state.connected) {
|
||||
cached_state.mode = AgvMode::Disconnected;
|
||||
}
|
||||
return cached_state;
|
||||
}
|
||||
|
||||
return queryRuntimeState_();
|
||||
}
|
||||
|
||||
AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const
|
||||
{
|
||||
AgvRuntimeState state;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
state.connected = connected_();
|
||||
state.last_error = last_error_;
|
||||
}
|
||||
state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected;
|
||||
|
||||
Json::Value loc;
|
||||
if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) {
|
||||
state.pose.x = jsonGet(loc, "x", 0.0).asDouble();
|
||||
state.pose.y = jsonGet(loc, "y", 0.0).asDouble();
|
||||
state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble();
|
||||
state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0;
|
||||
state.current_station = jsonGet(loc, "current_station", "").asString();
|
||||
}
|
||||
|
||||
Json::Value battery;
|
||||
if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) {
|
||||
state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble();
|
||||
state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble();
|
||||
state.battery.charging = jsonGet(battery, "charging", false).asBool();
|
||||
state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble();
|
||||
state.battery.current = jsonGet(battery, "current", 0.0).asDouble();
|
||||
}
|
||||
|
||||
Json::Value map;
|
||||
if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) {
|
||||
state.current_map = jsonGet(map, "current_map", "").asString();
|
||||
}
|
||||
|
||||
const auto nav = navigationStatus();
|
||||
state.moving = nav.state == AgvTaskState::Running;
|
||||
state.fault = nav.state == AgvTaskState::Failed;
|
||||
state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast<int>(nav.state));
|
||||
return state;
|
||||
}
|
||||
|
||||
AgvNavigationStatus SeerRobokitAgv::navigationStatus() const
|
||||
{
|
||||
AgvNavigationStatus status;
|
||||
std::string missing_pose_task_detail;
|
||||
for (int attempt = 0; attempt < 2; ++attempt) {
|
||||
PoseTaskContext pose_context;
|
||||
if (!currentPoseTask_(pose_context)) {
|
||||
break;
|
||||
}
|
||||
const auto observed_navigation_generation =
|
||||
navigation_generation_.load(std::memory_order_relaxed);
|
||||
if (pose_context.navigation_generation
|
||||
!= observed_navigation_generation) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PoseTaskStatus task_status;
|
||||
const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status);
|
||||
PoseTaskContext latest_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(latest_context)
|
||||
|| latest_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| latest_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
status.type = AgvTaskType::NavigateToPose;
|
||||
const auto fault_monitoring_unavailable =
|
||||
[this, &pose_context]() {
|
||||
if (controller_fault_channel_epoch_.load(
|
||||
std::memory_order_relaxed)
|
||||
!= pose_context
|
||||
.controller_fault_channel_epoch_at_start) {
|
||||
return std::string(
|
||||
"the controller fault push channel changed or was "
|
||||
"invalidated after the free-navigation command was "
|
||||
"accepted");
|
||||
}
|
||||
return freeNavigationFaultStateUnavailableDetail_();
|
||||
};
|
||||
if (!result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = result.message;
|
||||
return status;
|
||||
}
|
||||
if (!task_status.found || task_status.state == 404) {
|
||||
std::uint64_t missing_task_fault_control_attempt = 0;
|
||||
const std::string missing_task_fault =
|
||||
cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
controllerFaultCaptureGraceMs_(),
|
||||
&missing_task_fault_control_attempt);
|
||||
PoseTaskContext post_missing_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_missing_context)
|
||||
|| post_missing_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_missing_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
if (!missing_task_fault.empty()) {
|
||||
std::string attribution;
|
||||
if (missing_task_fault_control_attempt != 0
|
||||
&& missing_task_fault_control_attempt
|
||||
!= pose_context.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation task disappeared from "
|
||||
"1110 task_status_package while a new controller fault "
|
||||
"was observed: " + task_status.detail + ", "
|
||||
+ attribution + missing_task_fault;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
return status;
|
||||
}
|
||||
if (const std::string unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
!unavailable.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation status is unsafe to "
|
||||
"accept because controller fault monitoring is "
|
||||
"unavailable: " + unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
}
|
||||
missing_pose_task_detail = task_status.detail;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
break;
|
||||
}
|
||||
if (task_status.type_present && task_status.type != 1) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.type = toTaskType(task_status.type);
|
||||
status.message =
|
||||
"SEER Robokit returned an unexpected task type for the tracked "
|
||||
"free-navigation task: " + task_status.detail;
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
return status;
|
||||
}
|
||||
status.state = toTaskState(task_status.state);
|
||||
status.progress = task_status.progress;
|
||||
status.message = task_status.detail;
|
||||
const auto controller_reported_state = status.state;
|
||||
const bool controller_state_terminal =
|
||||
controller_reported_state == AgvTaskState::Completed
|
||||
|| controller_reported_state == AgvTaskState::Failed
|
||||
|| controller_reported_state == AgvTaskState::Canceled;
|
||||
std::uint64_t fault_control_attempt = 0;
|
||||
const std::string fault = cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
controller_reported_state == AgvTaskState::Completed
|
||||
|| controller_reported_state == AgvTaskState::Failed
|
||||
? controllerFaultCaptureGraceMs_()
|
||||
: 0,
|
||||
&fault_control_attempt);
|
||||
PoseTaskContext post_fault_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_fault_context)
|
||||
|| post_fault_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_fault_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
const std::string unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
if (!fault.empty()) {
|
||||
std::string attribution;
|
||||
if (fault_control_attempt != 0
|
||||
&& fault_control_attempt
|
||||
!= pose_context.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit reported a new controller fault while the tracked "
|
||||
"free-navigation task had controller_task_state="
|
||||
+ std::to_string(task_status.state) + ": "
|
||||
+ task_status.detail + ", " + attribution + fault;
|
||||
if (!unavailable.empty()) {
|
||||
status.message +=
|
||||
", controller_fault_monitoring_unavailable="
|
||||
+ unavailable;
|
||||
}
|
||||
} else if (!unavailable.empty()) {
|
||||
if (controller_reported_state == AgvTaskState::Failed
|
||||
|| controller_reported_state == AgvTaskState::Canceled) {
|
||||
status.message +=
|
||||
", controller_fault_monitoring_unavailable="
|
||||
+ unavailable;
|
||||
} else {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation state is unsafe to accept "
|
||||
"because controller fault monitoring became unavailable: "
|
||||
+ unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if (status.state == AgvTaskState::Completed) {
|
||||
std::string pose_detail;
|
||||
const bool target_reached =
|
||||
poseTargetReached_(pose_context, pose_detail);
|
||||
PoseTaskContext post_pose_context;
|
||||
if (navigation_generation_.load(std::memory_order_relaxed)
|
||||
!= observed_navigation_generation
|
||||
|| !currentPoseTask_(post_pose_context)
|
||||
|| post_pose_context.navigation_generation
|
||||
!= pose_context.navigation_generation
|
||||
|| post_pose_context.task_id != pose_context.task_id) {
|
||||
continue;
|
||||
}
|
||||
std::uint64_t post_pose_fault_control_attempt = 0;
|
||||
const std::string post_pose_fault =
|
||||
cachedControllerFaultDetail_(
|
||||
pose_context.controller_fault_sequence_at_start,
|
||||
0,
|
||||
&post_pose_fault_control_attempt);
|
||||
if (!post_pose_fault.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
std::string attribution;
|
||||
if (post_pose_fault_control_attempt != 0
|
||||
&& post_pose_fault_control_attempt
|
||||
!= pose_context
|
||||
.control_attempt_sequence_at_start) {
|
||||
attribution =
|
||||
"controller_fault_attribution=ambiguous because the "
|
||||
"fault was observed after another control command "
|
||||
"attempt had begun, ";
|
||||
}
|
||||
status.message =
|
||||
"SEER Robokit reported the tracked free-navigation task "
|
||||
"Completed, but a new controller fault was observed during "
|
||||
"target verification: " + task_status.detail + ", "
|
||||
+ attribution + post_pose_fault;
|
||||
} else if (const std::string post_pose_unavailable =
|
||||
fault_monitoring_unavailable();
|
||||
!post_pose_unavailable.empty()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit tracked free-navigation completion is unsafe to "
|
||||
"accept because controller fault monitoring became "
|
||||
"unavailable: " + post_pose_unavailable
|
||||
+ "; query the controller and cancel or stop before "
|
||||
"another motion command";
|
||||
return status;
|
||||
} else if (!target_reached) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message =
|
||||
"SEER Robokit reported the tracked free-navigation task "
|
||||
"Completed, but the requested target was not reached: "
|
||||
+ task_status.detail + ", " + pose_detail;
|
||||
} else {
|
||||
status.message += ", target_verified: " + pose_detail;
|
||||
}
|
||||
}
|
||||
if (controller_state_terminal) {
|
||||
clearPoseTask_(pose_context.navigation_generation);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
PoseTaskContext changed_context;
|
||||
if (currentPoseTask_(changed_context)) {
|
||||
status.state = AgvTaskState::Waiting;
|
||||
status.type = AgvTaskType::NavigateToPose;
|
||||
status.message =
|
||||
"SEER Robokit free-navigation task changed while its status was being "
|
||||
"queried; query navigation status again";
|
||||
return status;
|
||||
}
|
||||
|
||||
Json::Value payload(Json::objectValue);
|
||||
jsonMember(payload, "simple") = false;
|
||||
|
||||
Json::Value response;
|
||||
const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response);
|
||||
if (!result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = missing_pose_task_detail.empty()
|
||||
? result.message
|
||||
: missing_pose_task_detail + "; 1020 status query failed: "
|
||||
+ result.message;
|
||||
return status;
|
||||
}
|
||||
const auto controller_result = resultFromResponse_(response);
|
||||
if (!controller_result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = missing_pose_task_detail.empty()
|
||||
? controller_result.message
|
||||
: missing_pose_task_detail + "; 1020 status query failed: "
|
||||
+ controller_result.message;
|
||||
return status;
|
||||
}
|
||||
|
||||
status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
|
||||
status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
|
||||
status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString();
|
||||
if (!missing_pose_task_detail.empty()) {
|
||||
status.message = missing_pose_task_detail
|
||||
+ "; fallback_1020_status=" + std::to_string(
|
||||
jsonGet(response, "task_status", 0).asInt())
|
||||
+ ", fallback_1020_type=" + std::to_string(
|
||||
jsonGet(response, "task_type", 0).asInt())
|
||||
+ (status.message.empty() ? std::string{} : ", " + status.message);
|
||||
}
|
||||
if (const auto* task_status_package = jsonFind(response, "task_status_package")) {
|
||||
status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble();
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::configurePush_()
|
||||
{
|
||||
if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::InvalidArgument,
|
||||
"SEER Robokit push included_fields and excluded_fields cannot both be set");
|
||||
}
|
||||
|
||||
Json::Value payload(Json::objectValue);
|
||||
if (config_.state_push_interval_ms() > 0) {
|
||||
jsonMember(payload, "interval") = config_.state_push_interval_ms();
|
||||
}
|
||||
appendStringArray(payload, "included_fields", config_.state_push_included_fields());
|
||||
appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields());
|
||||
|
||||
if (payload.empty()) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
const std::string payload_text = toJsonString_(payload);
|
||||
const auto frame = buildFrame_(kRobotPushConfigReq, payload_text);
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock_push_ < 0) {
|
||||
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit push socket not connected");
|
||||
}
|
||||
if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit send push config failed: " + systemError());
|
||||
}
|
||||
|
||||
while (true) {
|
||||
std::uint16_t command = 0;
|
||||
std::string response_payload;
|
||||
const auto result = receiveFrame_(sock_push_, command, response_payload);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
|
||||
Json::Value response;
|
||||
std::string error;
|
||||
if (!response_payload.empty() && !parseJson_(response_payload, response, error)) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
|
||||
}
|
||||
|
||||
if (command == kRobotPushConfigRes) {
|
||||
return resultFromResponse_(response);
|
||||
}
|
||||
if (command == kRobotPush && response.isObject()) {
|
||||
updateCachedRuntimeState_(response);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::startPushThread_()
|
||||
{
|
||||
if (!state_push_enabled_) {
|
||||
return;
|
||||
}
|
||||
if (push_running_.exchange(true)) {
|
||||
return;
|
||||
}
|
||||
if (sock_push_ < 0) {
|
||||
push_running_ = false;
|
||||
return;
|
||||
}
|
||||
push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this);
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::stopPushThread_()
|
||||
{
|
||||
const bool was_running = push_running_.exchange(false);
|
||||
if (was_running) {
|
||||
int sock = -1;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
sock = sock_push_;
|
||||
}
|
||||
if (sock >= 0) {
|
||||
::shutdown(sock, SHUT_RDWR);
|
||||
}
|
||||
}
|
||||
if (push_thread_.joinable()) {
|
||||
push_thread_.join();
|
||||
}
|
||||
invalidateControllerFaultState_();
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::pushLoop_()
|
||||
{
|
||||
while (push_running_) {
|
||||
int sock = -1;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
sock = sock_push_;
|
||||
}
|
||||
if (sock < 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
continue;
|
||||
}
|
||||
|
||||
std::uint16_t command = 0;
|
||||
std::string payload;
|
||||
const auto result = receiveFrame_(sock, command, payload);
|
||||
if (!push_running_) {
|
||||
break;
|
||||
}
|
||||
if (!result.ok()) {
|
||||
if (result.code != AgvErrorCode::Timeout) {
|
||||
invalidateControllerFaultState_();
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
last_error_ = result.message;
|
||||
closeSocket_(sock_push_);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if (command != kRobotPush || payload.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Json::Value parsed;
|
||||
std::string error;
|
||||
if (!parseJson_(payload, parsed, error)) {
|
||||
invalidateControllerFaultState_();
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
last_error_ = error;
|
||||
continue;
|
||||
}
|
||||
updateCachedRuntimeState_(parsed);
|
||||
}
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::invalidateControllerFaultState_()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
controller_fault_channel_epoch_.fetch_add(
|
||||
1,
|
||||
std::memory_order_relaxed);
|
||||
controller_fault_state_observed_ = false;
|
||||
controller_fault_state_observed_at_ = {};
|
||||
active_controller_fault_detail_.clear();
|
||||
runtime_state_cv_.notify_all();
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{};
|
||||
state.timestamp = nowSeconds();
|
||||
state.connected = true;
|
||||
|
||||
if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble();
|
||||
if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble();
|
||||
if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble();
|
||||
if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble();
|
||||
if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble();
|
||||
if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble();
|
||||
if (jsonHas(payload, "battery_level")) {
|
||||
state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "battery_temp")) {
|
||||
state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "charging")) {
|
||||
state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool();
|
||||
}
|
||||
if (jsonHas(payload, "voltage")) {
|
||||
state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "current")) {
|
||||
state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble();
|
||||
}
|
||||
if (jsonHas(payload, "current_map")) {
|
||||
state.current_map = jsonGet(payload, "current_map", state.current_map).asString();
|
||||
}
|
||||
if (jsonHas(payload, "current_station")) {
|
||||
state.current_station = jsonGet(payload, "current_station", state.current_station).asString();
|
||||
}
|
||||
if (jsonHas(payload, "confidence")) {
|
||||
state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0;
|
||||
}
|
||||
if (jsonHas(payload, "emergency")) {
|
||||
state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool();
|
||||
}
|
||||
|
||||
state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4;
|
||||
const bool has_fatals = jsonHas(payload, "fatals");
|
||||
const bool has_errors = jsonHas(payload, "errors");
|
||||
const bool has_fault_fields = has_fatals || has_errors;
|
||||
if (has_fault_fields) {
|
||||
const auto* fatals = jsonFind(payload, "fatals");
|
||||
const auto* errors = jsonFind(payload, "errors");
|
||||
const bool valid_fatals = !has_fatals
|
||||
|| (fatals && fatals->isArray());
|
||||
const bool valid_errors = !has_errors
|
||||
|| (errors && errors->isArray());
|
||||
const bool complete_fault_state =
|
||||
has_fatals && has_errors && valid_fatals && valid_errors;
|
||||
if (complete_fault_state) {
|
||||
controller_fault_state_observed_ = true;
|
||||
controller_fault_state_observed_at_ =
|
||||
std::chrono::steady_clock::now();
|
||||
} else {
|
||||
controller_fault_state_observed_ = false;
|
||||
controller_fault_state_observed_at_ = {};
|
||||
}
|
||||
|
||||
const bool reported_fault =
|
||||
hasFaultArray(payload, "fatals")
|
||||
|| hasFaultArray(payload, "errors");
|
||||
const bool invalid_or_incomplete_fault_state =
|
||||
!complete_fault_state && !reported_fault;
|
||||
state.fault = reported_fault
|
||||
|| invalid_or_incomplete_fault_state;
|
||||
if (state.fault) {
|
||||
std::ostringstream detail;
|
||||
detail << (reported_fault
|
||||
? "SEER Robokit controller fault"
|
||||
: "SEER Robokit controller fault state is incomplete or malformed");
|
||||
if (fatals
|
||||
&& (!fatals->isArray()
|
||||
|| !fatals->empty()
|
||||
|| !complete_fault_state)) {
|
||||
detail << ": fatals="
|
||||
<< (fatals->isNull()
|
||||
? std::string("null")
|
||||
: jsonValueToString(*fatals));
|
||||
}
|
||||
if (errors
|
||||
&& (!errors->isArray()
|
||||
|| !errors->empty()
|
||||
|| !complete_fault_state)) {
|
||||
detail << ": errors="
|
||||
<< (errors->isNull()
|
||||
? std::string("null")
|
||||
: jsonValueToString(*errors));
|
||||
}
|
||||
state.last_error = detail.str();
|
||||
if (state.last_error != active_controller_fault_detail_) {
|
||||
active_controller_fault_detail_ = state.last_error;
|
||||
++controller_fault_sequence_;
|
||||
last_controller_fault_timestamp_ = state.timestamp;
|
||||
last_controller_fault_detail_ = state.last_error;
|
||||
last_controller_fault_control_attempt_ =
|
||||
control_attempt_sequence_.load(
|
||||
std::memory_order_acquire);
|
||||
}
|
||||
} else {
|
||||
state.last_error.clear();
|
||||
active_controller_fault_detail_.clear();
|
||||
}
|
||||
}
|
||||
if (state.emergency_stopped) {
|
||||
state.mode = AgvMode::EmergencyStop;
|
||||
} else if (state.fault) {
|
||||
state.mode = AgvMode::Fault;
|
||||
} else if (state.battery.charging) {
|
||||
state.mode = AgvMode::Charging;
|
||||
} else if (state.moving) {
|
||||
state.mode = AgvMode::Auto;
|
||||
} else {
|
||||
state.mode = AgvMode::Idle;
|
||||
}
|
||||
|
||||
cached_runtime_state_ = state;
|
||||
cached_runtime_state_valid_ = true;
|
||||
runtime_state_cv_.notify_all();
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
362
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp
Normal file
362
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp
Normal file
@ -0,0 +1,362 @@
|
||||
#include "seer_robokit_agv.h"
|
||||
#include "seer_robokit_protocol.h"
|
||||
#include "seer_robokit_utils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <arpa/inet.h>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
using namespace seer_robokit::protocol;
|
||||
using namespace seer_robokit::detail;
|
||||
|
||||
AgvResult SeerRobokitAgv::connectSocket_(int& sock, const int port)
|
||||
{
|
||||
sock = ::socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock < 0) {
|
||||
last_error_ = "create socket failed: " + systemError();
|
||||
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
|
||||
}
|
||||
|
||||
sockaddr_in address{};
|
||||
address.sin_family = AF_INET;
|
||||
address.sin_port = htons(static_cast<std::uint16_t>(port));
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) {
|
||||
closeSocket_(sock);
|
||||
last_error_ = "invalid SEER Robokit ip: " + ip_;
|
||||
return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_);
|
||||
}
|
||||
|
||||
if (::connect(sock, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) {
|
||||
closeSocket_(sock);
|
||||
last_error_ = "connect SEER Robokit port " + std::to_string(port) + " failed: " + systemError();
|
||||
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
|
||||
}
|
||||
|
||||
timeval timeout{};
|
||||
timeout.tv_sec = recv_timeout_ms_ / 1000;
|
||||
timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000;
|
||||
::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::ensureOtherSocket_()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock_other_ >= 0) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
return connectSocket_(sock_other_, ports_.other);
|
||||
}
|
||||
|
||||
void SeerRobokitAgv::closeSocket_(int& sock) const
|
||||
{
|
||||
if (sock >= 0) {
|
||||
::close(sock);
|
||||
sock = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::connected_() const
|
||||
{
|
||||
return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommand_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response,
|
||||
CommandTransmissionState* transmission_state) const
|
||||
{
|
||||
std::string response_payload;
|
||||
auto result = sendCommandRaw_(
|
||||
sock,
|
||||
command,
|
||||
payload,
|
||||
&response_payload,
|
||||
transmission_state);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
if (!response) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
Json::Value parsed;
|
||||
std::string error;
|
||||
if (!parseJson_(response_payload, parsed, error)) {
|
||||
const std::string json_text = extractJson_(response_payload);
|
||||
if (json_text.empty() || !parseJson_(json_text, parsed, error)) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
|
||||
}
|
||||
}
|
||||
|
||||
*response = std::move(parsed);
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommandRaw_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload,
|
||||
CommandTransmissionState* transmission_state) const
|
||||
{
|
||||
if (transmission_state) {
|
||||
*transmission_state = CommandTransmissionState::NotSent;
|
||||
}
|
||||
const auto exchange = [&]() {
|
||||
const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
|
||||
const auto frame = buildFrame_(command, payload_text);
|
||||
const auto sent = ::send(
|
||||
sock,
|
||||
frame.data(),
|
||||
frame.size(),
|
||||
MSG_NOSIGNAL);
|
||||
if (sent > 0 && transmission_state) {
|
||||
*transmission_state = CommandTransmissionState::PossiblySent;
|
||||
}
|
||||
if (sent != static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit send command failed: " + systemError());
|
||||
}
|
||||
|
||||
std::uint16_t response_command = 0;
|
||||
std::string payload_text_response;
|
||||
const auto result = receiveFrame_(sock, response_command, payload_text_response);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
const auto expected_response_command = static_cast<std::uint16_t>(
|
||||
command + 10000U);
|
||||
if (response_command != expected_response_command) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit response command mismatch: expected="
|
||||
+ std::to_string(expected_response_command)
|
||||
+ ", actual=" + std::to_string(response_command));
|
||||
}
|
||||
if (response_payload) {
|
||||
*response_payload = std::move(payload_text_response);
|
||||
}
|
||||
return AgvResult::success();
|
||||
};
|
||||
const auto close_matching_socket_locked = [this, sock]() {
|
||||
if (sock == sock_status_) {
|
||||
closeSocket_(sock_status_);
|
||||
} else if (sock == sock_control_) {
|
||||
closeSocket_(sock_control_);
|
||||
} else if (sock == sock_navigation_) {
|
||||
closeSocket_(sock_navigation_);
|
||||
} else if (sock == sock_config_) {
|
||||
closeSocket_(sock_config_);
|
||||
} else if (sock == sock_other_) {
|
||||
closeSocket_(sock_other_);
|
||||
}
|
||||
};
|
||||
const auto mark_channel_desynchronized = [](AgvResult result) {
|
||||
std::string detail = result.message.empty()
|
||||
? "unknown transport or frame error"
|
||||
: result.message;
|
||||
detail +=
|
||||
"; SEER Robokit channel closed because the response stream may be "
|
||||
"desynchronized; reconnect before sending another command";
|
||||
return AgvResult::failure(result.code, detail);
|
||||
};
|
||||
|
||||
bool is_status_socket = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit socket not connected");
|
||||
}
|
||||
is_status_socket = sock == sock_status_;
|
||||
}
|
||||
|
||||
if (is_status_socket) {
|
||||
// A slow 1110 status response must never hold the lifecycle/global I/O
|
||||
// mutex needed by cancelNavigation() or emergencyStop(). The dedicated
|
||||
// status lock still serializes requests on port 19204. connect_() and
|
||||
// disconnect_() take this lock before changing the descriptor.
|
||||
std::lock_guard<std::mutex> status_lock(status_io_mutex_);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0 || sock != sock_status_) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit status socket is no longer connected");
|
||||
}
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0
|
||||
|| (sock != sock_control_
|
||||
&& sock != sock_navigation_
|
||||
&& sock != sock_config_
|
||||
&& sock != sock_other_)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SEER Robokit socket is no longer connected");
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::sendCommandNoResponse_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload) const
|
||||
{
|
||||
return sendCommand_(sock, command, payload, nullptr);
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> SeerRobokitAgv::buildFrame_(
|
||||
const std::uint16_t command,
|
||||
const std::string& payload)
|
||||
{
|
||||
std::vector<std::uint8_t> frame(16 + payload.size(), 0);
|
||||
frame[0] = 0x5A;
|
||||
frame[1] = 0x01;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = 0x01;
|
||||
const auto length = static_cast<std::uint32_t>(payload.size());
|
||||
frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
|
||||
frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
|
||||
frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
|
||||
frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
|
||||
frame[8] = static_cast<std::uint8_t>((command >> 8U) & 0xFFU);
|
||||
frame[9] = static_cast<std::uint8_t>(command & 0xFFU);
|
||||
std::copy(payload.begin(), payload.end(), frame.begin() + 16);
|
||||
return frame;
|
||||
}
|
||||
|
||||
std::string SeerRobokitAgv::toJsonString_(const Json::Value& value)
|
||||
{
|
||||
Json::StreamWriterBuilder builder;
|
||||
builder["indentation"] = "";
|
||||
return Json::writeString(builder, value);
|
||||
}
|
||||
|
||||
bool SeerRobokitAgv::parseJson_(const std::string& input, Json::Value& output, std::string& error)
|
||||
{
|
||||
Json::CharReaderBuilder builder;
|
||||
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
|
||||
return reader->parse(input.data(), input.data() + input.size(), &output, &error);
|
||||
}
|
||||
|
||||
std::string SeerRobokitAgv::extractJson_(const std::string& raw)
|
||||
{
|
||||
const auto begin = raw.find('{');
|
||||
const auto end = raw.rfind('}');
|
||||
if (begin == std::string::npos || end == std::string::npos || end < begin) {
|
||||
return {};
|
||||
}
|
||||
return raw.substr(begin, end - begin + 1);
|
||||
}
|
||||
|
||||
AgvResult SeerRobokitAgv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload)
|
||||
{
|
||||
const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult {
|
||||
std::size_t offset = 0;
|
||||
while (offset < size) {
|
||||
const ssize_t count = ::recv(fd, data + offset, size - offset, 0);
|
||||
if (count > 0) {
|
||||
offset += static_cast<std::size_t>(count);
|
||||
continue;
|
||||
}
|
||||
if (count == 0) {
|
||||
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit socket closed");
|
||||
}
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
||||
return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout");
|
||||
}
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit receive failed: " + systemError());
|
||||
}
|
||||
return AgvResult::success();
|
||||
};
|
||||
|
||||
std::uint8_t header[16]{};
|
||||
auto result = recv_exact(sock, header, sizeof(header));
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
if (header[0] != 0x5A) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame header is invalid");
|
||||
}
|
||||
|
||||
const auto length = (static_cast<std::uint32_t>(header[4]) << 24U)
|
||||
| (static_cast<std::uint32_t>(header[5]) << 16U)
|
||||
| (static_cast<std::uint32_t>(header[6]) << 8U)
|
||||
| static_cast<std::uint32_t>(header[7]);
|
||||
command = static_cast<std::uint16_t>((static_cast<std::uint16_t>(header[8]) << 8U) | header[9]);
|
||||
payload.clear();
|
||||
if (length == 0) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
if (length > kMaxFramePayloadBytes) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame payload is too large");
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> buffer(length);
|
||||
result = recv_exact(sock, buffer.data(), buffer.size());
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
payload.assign(reinterpret_cast<const char*>(buffer.data()), buffer.size());
|
||||
return AgvResult::success();
|
||||
}
|
||||
|
||||
|
||||
AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response)
|
||||
{
|
||||
if (!hasNumericControllerRetCode(response)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SEER Robokit controller response is missing a numeric ret_code");
|
||||
}
|
||||
const auto* ret_code_value = jsonFind(response, "ret_code");
|
||||
const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
|
||||
? ret_code_value->asUInt64() == 0
|
||||
: ret_code_value->asInt64() == 0;
|
||||
const std::string ret_code = jsonValueToString(*ret_code_value);
|
||||
const std::string message = jsonGet(response, "err_msg", "").asString();
|
||||
if (success) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
std::string detail = "SEER Robokit command failed: ret_code=" + ret_code;
|
||||
if (!message.empty()) {
|
||||
detail += ", err_msg=" + message;
|
||||
}
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, detail);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,40 +0,0 @@
|
||||
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)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
add_executable(src1100_control_authority_test
|
||||
tests/src1100_control_authority_test.cpp
|
||||
)
|
||||
target_link_libraries(src1100_control_authority_test
|
||||
PRIVATE
|
||||
cmvr_es::device::src1100_agv
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
)
|
||||
add_test(
|
||||
NAME src1100_control_authority_test
|
||||
COMMAND src1100_control_authority_test
|
||||
)
|
||||
set(_src1100_control_authority_test_environment
|
||||
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
|
||||
)
|
||||
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
|
||||
list(APPEND _src1100_control_authority_test_environment
|
||||
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
|
||||
endif()
|
||||
set_tests_properties(src1100_control_authority_test PROPERTIES
|
||||
TIMEOUT 10
|
||||
ENVIRONMENT "${_src1100_control_authority_test_environment}"
|
||||
)
|
||||
endif()
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -27,7 +27,24 @@ grpc::Status resultToStatus(const device::AgvResult& result)
|
||||
if (result.ok()) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
|
||||
switch (result.code) {
|
||||
case device::AgvErrorCode::InvalidArgument:
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::INVALID_ARGUMENT,
|
||||
result.message);
|
||||
case device::AgvErrorCode::TaskCanceled:
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::CANCELLED,
|
||||
result.message);
|
||||
case device::AgvErrorCode::Timeout:
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::DEADLINE_EXCEEDED,
|
||||
result.message);
|
||||
default:
|
||||
return grpc::Status(
|
||||
grpc::StatusCode::INTERNAL,
|
||||
result.message);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
@ -58,6 +75,15 @@ grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std:
|
||||
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||
}
|
||||
|
||||
template <typename Response>
|
||||
grpc::Status setNavigationRequestCanceled(Response* response)
|
||||
{
|
||||
constexpr char message[] =
|
||||
"AGV navigation request was canceled before command dispatch";
|
||||
fillFeedback(response->mutable_header(), false, message);
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, message);
|
||||
}
|
||||
|
||||
device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src)
|
||||
{
|
||||
device::AgvAdapterParams dst;
|
||||
@ -67,7 +93,9 @@ device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src)
|
||||
return dst;
|
||||
}
|
||||
|
||||
device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src)
|
||||
device::AgvMotionOptions toMotionOptions(
|
||||
const msgs::AgvMotionOptions& src,
|
||||
grpc::ServerContext* context = nullptr)
|
||||
{
|
||||
device::AgvMotionOptions dst;
|
||||
dst.max_speed = src.max_speed();
|
||||
@ -78,6 +106,13 @@ device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src)
|
||||
dst.reach_angle = src.reach_angle();
|
||||
dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0;
|
||||
dst.asynchronous = src.asynchronous();
|
||||
dst.wait_timeout_ms = src.wait_timeout_ms();
|
||||
dst.poll_interval_ms = src.poll_interval_ms();
|
||||
if (context) {
|
||||
dst.cancellation_requested = [context]() {
|
||||
return context->IsCancelled();
|
||||
};
|
||||
}
|
||||
return dst;
|
||||
}
|
||||
|
||||
@ -391,11 +426,14 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*,
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*,
|
||||
grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context,
|
||||
const api::AgvNavigateToPoseCommand_Request* request,
|
||||
api::AgvNavigateToPoseCommand_Feedback* response)
|
||||
{
|
||||
try {
|
||||
if (context && context->IsCancelled()) {
|
||||
return setNavigationRequestCanceled(response);
|
||||
}
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||
if (!agv) {
|
||||
@ -403,7 +441,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*,
|
||||
}
|
||||
return setResponseResult(response, agv->navigateToPose(
|
||||
toPose2d(request->pose()),
|
||||
toMotionOptions(request->options()),
|
||||
toMotionOptions(request->options(), context),
|
||||
toAdapterParams(request->adapter_params())));
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
@ -411,11 +449,14 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*,
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*,
|
||||
grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context,
|
||||
const api::AgvNavigateToStationCommand_Request* request,
|
||||
api::AgvNavigateToStationCommand_Feedback* response)
|
||||
{
|
||||
try {
|
||||
if (context && context->IsCancelled()) {
|
||||
return setNavigationRequestCanceled(response);
|
||||
}
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||
if (!agv) {
|
||||
@ -423,7 +464,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*,
|
||||
}
|
||||
return setResponseResult(response, agv->navigateToStation(
|
||||
request->station_id(),
|
||||
toMotionOptions(request->options()),
|
||||
toMotionOptions(request->options(), context),
|
||||
toAdapterParams(request->adapter_params())));
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
@ -431,11 +472,14 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*,
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*,
|
||||
grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context,
|
||||
const api::AgvFollowPathCommand_Request* request,
|
||||
api::AgvFollowPathCommand_Feedback* response)
|
||||
{
|
||||
try {
|
||||
if (context && context->IsCancelled()) {
|
||||
return setNavigationRequestCanceled(response);
|
||||
}
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||
if (!agv) {
|
||||
@ -446,7 +490,11 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*,
|
||||
for (const auto& segment : request->path()) {
|
||||
path.push_back(toPathSegment(segment));
|
||||
}
|
||||
return setResponseResult(response, agv->followPath(path));
|
||||
return setResponseResult(
|
||||
response,
|
||||
agv->followPath(
|
||||
path,
|
||||
toMotionOptions(request->options(), context)));
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include <gtest/gtest.h>
|
||||
@ -13,9 +14,9 @@ namespace cmvr::service {
|
||||
namespace {
|
||||
|
||||
constexpr char kNativeErrorMessage[] =
|
||||
"SRC1100 command failed: ret_code=41200, err_msg=speed_illegal";
|
||||
"SEER Robokit command failed: ret_code=41200, err_msg=speed_illegal";
|
||||
constexpr char kNativeNavigationErrorMessage[] =
|
||||
"SRC1100 command failed: ret_code=43051, err_msg=planner_rejected_pose";
|
||||
"SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose";
|
||||
|
||||
class FakeAgv final : public device::AbstractAGV {
|
||||
public:
|
||||
@ -33,16 +34,41 @@ public:
|
||||
{
|
||||
pose_ = pose;
|
||||
pose_options_ = options;
|
||||
pose_cancellation_bound_ =
|
||||
static_cast<bool>(options.cancellation_requested);
|
||||
pose_cancellation_requested_during_call_ =
|
||||
pose_cancellation_bound_ && options.cancellation_requested();
|
||||
pose_options_.cancellation_requested = {};
|
||||
return pose_result_;
|
||||
}
|
||||
|
||||
device::AgvResult navigateToStation(
|
||||
const std::string& station_id,
|
||||
const device::AgvMotionOptions& options,
|
||||
const device::AgvAdapterParams&) override
|
||||
const device::AgvAdapterParams& adapter_params) override
|
||||
{
|
||||
station_id_ = station_id;
|
||||
station_options_ = options;
|
||||
station_adapter_params_ = adapter_params;
|
||||
station_cancellation_bound_ =
|
||||
static_cast<bool>(options.cancellation_requested);
|
||||
station_cancellation_requested_during_call_ =
|
||||
station_cancellation_bound_ && options.cancellation_requested();
|
||||
station_options_.cancellation_requested = {};
|
||||
return device::AgvResult::success();
|
||||
}
|
||||
|
||||
device::AgvResult followPath(
|
||||
const std::vector<device::AgvPathSegment>& path,
|
||||
const device::AgvMotionOptions& options) override
|
||||
{
|
||||
path_ = path;
|
||||
path_options_ = options;
|
||||
path_cancellation_bound_ =
|
||||
static_cast<bool>(options.cancellation_requested);
|
||||
path_cancellation_requested_during_call_ =
|
||||
path_cancellation_bound_ && options.cancellation_requested();
|
||||
path_options_.cancellation_requested = {};
|
||||
return device::AgvResult::success();
|
||||
}
|
||||
|
||||
@ -58,6 +84,29 @@ public:
|
||||
device::AgvResult pose_result_{device::AgvResult::success()};
|
||||
std::string station_id_;
|
||||
device::AgvMotionOptions station_options_;
|
||||
device::AgvAdapterParams station_adapter_params_;
|
||||
std::vector<device::AgvPathSegment> path_;
|
||||
device::AgvMotionOptions path_options_;
|
||||
bool pose_cancellation_bound_{false};
|
||||
bool pose_cancellation_requested_during_call_{false};
|
||||
bool station_cancellation_bound_{false};
|
||||
bool station_cancellation_requested_during_call_{false};
|
||||
bool path_cancellation_bound_{false};
|
||||
bool path_cancellation_requested_during_call_{false};
|
||||
};
|
||||
|
||||
class LegacyFollowPathAgv final : public device::AbstractAGV {
|
||||
public:
|
||||
std::string typeName() const override { return "LegacyFollowPathAgv"; }
|
||||
|
||||
device::AgvResult followPath(
|
||||
const std::vector<device::AgvPathSegment>& path) override
|
||||
{
|
||||
path_ = path;
|
||||
return device::AgvResult::success();
|
||||
}
|
||||
|
||||
std::vector<device::AgvPathSegment> path_;
|
||||
};
|
||||
|
||||
class GrpcAgvServiceTest : public ::testing::Test {
|
||||
@ -90,6 +139,8 @@ void setMotionOptions(msgs::AgvMotionOptions* options)
|
||||
options->set_max_angular_acceleration(0.7);
|
||||
options->set_reach_distance(0.08);
|
||||
options->set_reach_angle(0.09);
|
||||
options->set_wait_timeout_ms(1234);
|
||||
options->set_poll_interval_ms(55);
|
||||
}
|
||||
|
||||
void expectMotionOptions(const device::AgvMotionOptions& options)
|
||||
@ -100,6 +151,9 @@ void expectMotionOptions(const device::AgvMotionOptions& options)
|
||||
EXPECT_DOUBLE_EQ(options.max_angular_acceleration, 0.7);
|
||||
EXPECT_DOUBLE_EQ(options.reach_distance, 0.08);
|
||||
EXPECT_DOUBLE_EQ(options.reach_angle, 0.09);
|
||||
EXPECT_EQ(options.wait_timeout_ms, 1234);
|
||||
EXPECT_EQ(options.poll_interval_ms, 55);
|
||||
EXPECT_FALSE(options.asynchronous);
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions)
|
||||
@ -124,6 +178,8 @@ TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions)
|
||||
EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0);
|
||||
EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5);
|
||||
expectMotionOptions(agv_->pose_options_);
|
||||
EXPECT_TRUE(agv_->pose_cancellation_bound_);
|
||||
EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_);
|
||||
|
||||
api::AgvNavigateToStationCommand_Request station_request;
|
||||
station_request.mutable_header()->set_device_id("test-agv");
|
||||
@ -141,6 +197,31 @@ TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions)
|
||||
EXPECT_TRUE(station_response.header().success());
|
||||
EXPECT_EQ(agv_->station_id_, "station-1");
|
||||
expectMotionOptions(agv_->station_options_);
|
||||
EXPECT_TRUE(agv_->station_cancellation_bound_);
|
||||
EXPECT_FALSE(agv_->station_cancellation_requested_during_call_);
|
||||
|
||||
api::AgvFollowPathCommand_Request path_request;
|
||||
path_request.mutable_header()->set_device_id("test-agv");
|
||||
auto* segment = path_request.add_path();
|
||||
segment->set_source_station("station-1");
|
||||
segment->set_target_station("station-2");
|
||||
setMotionOptions(path_request.mutable_options());
|
||||
api::AgvFollowPathCommand_Feedback path_response;
|
||||
grpc::ServerContext path_context;
|
||||
|
||||
const auto path_status = service_->followPath(
|
||||
&path_context,
|
||||
&path_request,
|
||||
&path_response);
|
||||
|
||||
ASSERT_TRUE(path_status.ok()) << path_status.error_message();
|
||||
EXPECT_TRUE(path_response.header().success());
|
||||
ASSERT_EQ(agv_->path_.size(), 1U);
|
||||
EXPECT_EQ(agv_->path_[0].source_station, "station-1");
|
||||
EXPECT_EQ(agv_->path_[0].target_station, "station-2");
|
||||
expectMotionOptions(agv_->path_options_);
|
||||
EXPECT_TRUE(agv_->path_cancellation_bound_);
|
||||
EXPECT_FALSE(agv_->path_cancellation_requested_during_call_);
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage)
|
||||
@ -185,6 +266,122 @@ TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage)
|
||||
EXPECT_EQ(
|
||||
response.header().error_message(),
|
||||
kNativeNavigationErrorMessage);
|
||||
EXPECT_FALSE(agv_->pose_options_.asynchronous);
|
||||
EXPECT_EQ(agv_->pose_options_.wait_timeout_ms, 0);
|
||||
EXPECT_EQ(agv_->pose_options_.poll_interval_ms, 0);
|
||||
EXPECT_TRUE(agv_->pose_cancellation_bound_);
|
||||
EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_);
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded)
|
||||
{
|
||||
api::AgvNavigateToStationCommand_Request request;
|
||||
request.mutable_header()->set_device_id("test-agv");
|
||||
request.set_station_id("station-async");
|
||||
request.mutable_options()->set_asynchronous(true);
|
||||
api::AgvNavigateToStationCommand_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
|
||||
const auto status = service_->navigateToStation(
|
||||
&context,
|
||||
&request,
|
||||
&response);
|
||||
|
||||
ASSERT_TRUE(status.ok()) << status.error_message();
|
||||
EXPECT_TRUE(agv_->station_options_.asynchronous);
|
||||
EXPECT_TRUE(agv_->station_cancellation_bound_);
|
||||
EXPECT_FALSE(agv_->station_cancellation_requested_during_call_);
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams)
|
||||
{
|
||||
api::AgvNavigateToStationCommand_Request request;
|
||||
request.mutable_header()->set_device_id("test-agv");
|
||||
request.set_station_id("AP1");
|
||||
auto* values = request.mutable_adapter_params()->mutable_values();
|
||||
(*values)["use_pgv"] = "true";
|
||||
(*values)["pgv_adjust_dist"] = "0.3";
|
||||
(*values)["pgv_adjust_cx"] = "-0.3";
|
||||
(*values)["pgv_adjust_cy"] = "0";
|
||||
api::AgvNavigateToStationCommand_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
|
||||
const auto status = service_->navigateToStation(
|
||||
&context,
|
||||
&request,
|
||||
&response);
|
||||
|
||||
ASSERT_TRUE(status.ok()) << status.error_message();
|
||||
EXPECT_TRUE(response.header().success());
|
||||
EXPECT_EQ(agv_->station_id_, "AP1");
|
||||
EXPECT_EQ(
|
||||
agv_->station_adapter_params_.getString("use_pgv").value_or(""),
|
||||
"true");
|
||||
EXPECT_EQ(
|
||||
agv_->station_adapter_params_.getString("pgv_adjust_dist").value_or(""),
|
||||
"0.3");
|
||||
EXPECT_EQ(
|
||||
agv_->station_adapter_params_.getString("pgv_adjust_cx").value_or(""),
|
||||
"-0.3");
|
||||
EXPECT_EQ(
|
||||
agv_->station_adapter_params_.getString("pgv_adjust_cy").value_or(""),
|
||||
"0");
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, NavigationErrorsMapToGrpcCodesAndPreserveDetails)
|
||||
{
|
||||
struct ErrorCase {
|
||||
device::AgvErrorCode device_code;
|
||||
grpc::StatusCode grpc_code;
|
||||
};
|
||||
const ErrorCase cases[] = {
|
||||
{device::AgvErrorCode::InvalidArgument,
|
||||
grpc::StatusCode::INVALID_ARGUMENT},
|
||||
{device::AgvErrorCode::TaskCanceled,
|
||||
grpc::StatusCode::CANCELLED},
|
||||
{device::AgvErrorCode::Timeout,
|
||||
grpc::StatusCode::DEADLINE_EXCEEDED},
|
||||
};
|
||||
|
||||
for (const auto& test_case : cases) {
|
||||
const std::string detail =
|
||||
"SEER Robokit navigation detail for code="
|
||||
+ std::to_string(static_cast<int>(test_case.device_code));
|
||||
agv_->pose_result_ = device::AgvResult::failure(
|
||||
test_case.device_code,
|
||||
detail);
|
||||
api::AgvNavigateToPoseCommand_Request request;
|
||||
request.mutable_header()->set_device_id("test-agv");
|
||||
api::AgvNavigateToPoseCommand_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
|
||||
const auto status = service_->navigateToPose(
|
||||
&context,
|
||||
&request,
|
||||
&response);
|
||||
|
||||
EXPECT_EQ(status.error_code(), test_case.grpc_code);
|
||||
EXPECT_EQ(status.error_message(), detail);
|
||||
EXPECT_FALSE(response.header().success());
|
||||
EXPECT_EQ(response.header().error_message(), detail);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride)
|
||||
{
|
||||
LegacyFollowPathAgv legacy;
|
||||
device::AbstractAGV* abstract = &legacy;
|
||||
const std::vector<device::AgvPathSegment> path = {
|
||||
{"station-1", "station-2"},
|
||||
};
|
||||
device::AgvMotionOptions options;
|
||||
|
||||
const auto result = abstract->followPath(path, options);
|
||||
|
||||
ASSERT_TRUE(result.ok()) << result.message;
|
||||
ASSERT_EQ(legacy.path_.size(), 1U);
|
||||
EXPECT_EQ(legacy.path_[0].source_station, "station-1");
|
||||
EXPECT_EQ(legacy.path_[0].target_station, "station-2");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
@ -630,7 +630,7 @@ bool testDeviceManagerSnapshotInHeartbeat()
|
||||
device::ManagedDeviceSnapshot running;
|
||||
running.id = "src1100";
|
||||
running.kind = device::DeviceKind::AGV;
|
||||
running.type_name = "Src1100Agv";
|
||||
running.type_name = "SeerRobokitAgv";
|
||||
running.enabled = true;
|
||||
running.state = device::ManagedDeviceState::Running;
|
||||
running.health.state = device::DeviceHealthState::Healthy;
|
||||
|
||||
@ -45,7 +45,7 @@ message AgvNavigateToPoseCommand {
|
||||
CommandHeader.Request header = 1;
|
||||
// 目标位姿。x/y 单位:米,theta 单位:弧度。
|
||||
cmvr.msgs.AgvPose2d pose = 2;
|
||||
// 通用运动约束和执行选项。
|
||||
// 通用运动约束和执行选项;默认同步阻塞至任务终态并确认停车。
|
||||
cmvr.msgs.AgvMotionOptions options = 3;
|
||||
// AGV 适配器扩展参数,用于传递厂商特有选项。
|
||||
cmvr.msgs.AgvAdapterParams adapter_params = 4;
|
||||
@ -65,7 +65,7 @@ message AgvNavigateToStationCommand {
|
||||
CommandHeader.Request header = 1;
|
||||
// 目标站点 id。
|
||||
string station_id = 2;
|
||||
// 通用运动约束和执行选项。
|
||||
// 通用运动约束和执行选项;默认同步阻塞至任务终态并确认停车。
|
||||
cmvr.msgs.AgvMotionOptions options = 3;
|
||||
// AGV 适配器扩展参数,用于传递厂商特有选项。
|
||||
cmvr.msgs.AgvAdapterParams adapter_params = 4;
|
||||
@ -85,6 +85,8 @@ message AgvFollowPathCommand {
|
||||
CommandHeader.Request header = 1;
|
||||
// 路径段列表。每段包含起点站点 id 和终点站点 id。
|
||||
repeated cmvr.msgs.AgvPathSegment path = 2;
|
||||
// 通用执行选项;默认同步阻塞至整条路径终态并确认停车。
|
||||
cmvr.msgs.AgvMotionOptions options = 3;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
|
||||
@ -11,11 +11,11 @@ message MyAgvConfig {
|
||||
int32 port = 3;
|
||||
}
|
||||
|
||||
// 仙工 SRC1100 AGV 后端配置。
|
||||
message Src1100AgvConfig {
|
||||
// 仙工 SEER Robokit AGV 后端配置。
|
||||
message SeerRobokitAgvConfig {
|
||||
// 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。
|
||||
string id = 1;
|
||||
// SRC1100 控制器 IP 地址。
|
||||
// SEER Robokit 控制器 IP 地址。
|
||||
string ip = 2;
|
||||
// 是否启用该后端配置。当前设备是否创建仍以设备管理器配置为准。
|
||||
bool enable = 3;
|
||||
@ -49,7 +49,7 @@ message Src1100AgvConfig {
|
||||
int32 map_update_interval_ms = 17;
|
||||
// 统一地图更新缓存条数。0 表示使用适配器默认值;缓存满后会丢弃最旧更新。
|
||||
uint32 map_update_history_size = 18;
|
||||
// 抢占 SRC1100 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:<device-id>"。
|
||||
// 抢占 SEER Robokit 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:<device-id>"。
|
||||
string control_nick_name = 19;
|
||||
}
|
||||
|
||||
@ -62,8 +62,8 @@ message AGVDeviceConfig {
|
||||
oneof backend {
|
||||
// 示例/测试 AGV 后端。
|
||||
MyAgvConfig my_agv = 10;
|
||||
// 仙工 SRC1100 AGV 后端。
|
||||
Src1100AgvConfig src1100_agv = 11;
|
||||
// 仙工 SEER Robokit AGV 后端。
|
||||
SeerRobokitAgvConfig seer_robokit_agv = 11;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -52,8 +52,14 @@ message AgvMotionOptions {
|
||||
double reach_angle = 6;
|
||||
// 速度比例,范围通常为 [0, 1];1 表示不降速。
|
||||
double speed_ratio = 7;
|
||||
// 是否异步执行;true 表示下发任务后立即返回。
|
||||
// 是否异步执行;false(默认)表示到达、失败、取消或遇障停止后才返回,
|
||||
// true 表示任务被控制器接受后立即返回。
|
||||
bool asynchronous = 8;
|
||||
// 同步导航的最大等待时间,单位:毫秒;0 表示使用适配器默认值。
|
||||
// gRPC deadline 应大于该值或预计行程时间,否则服务端会安全取消导航。
|
||||
int32 wait_timeout_ms = 9;
|
||||
// 同步导航的状态轮询周期,单位:毫秒;0 表示使用适配器默认值。
|
||||
int32 poll_interval_ms = 10;
|
||||
}
|
||||
|
||||
// AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。
|
||||
|
||||
@ -2,7 +2,7 @@ syntax = "proto3";
|
||||
|
||||
package rbk.protocol;
|
||||
|
||||
// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。
|
||||
// 仙工 SEER Robokit 3D 地图文件 0.3dsmap 的最小解析结构。
|
||||
// 这里只保留转换统一地图所需字段,未声明字段由 protobuf 作为未知字段跳过。
|
||||
|
||||
// 地图坐标系下的三维位置,单位:米。
|
||||
Loading…
Reference in New Issue
Block a user