feat: extend and refactor SEER Robokit AGV backend

This commit is contained in:
xtkuang 2026-08-03 15:05:04 +08:00
parent 26a7ad5d4b
commit 52d30ee412
32 changed files with 12384 additions and 6440 deletions

View File

@ -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;

View File

@ -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

View File

@ -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
}

View File

@ -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 |

View File

@ -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
)

View File

@ -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

View File

@ -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:

View 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()

View 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。

View File

@ -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

View File

@ -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

View File

@ -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, &params](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, &params](
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

View File

@ -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

View File

@ -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

View 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

View 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

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View 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

View 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

View File

@ -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

View File

@ -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());

View File

@ -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

View File

@ -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;

View File

@ -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 {

View File

@ -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;
}
}

View File

@ -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

View File

@ -2,7 +2,7 @@ syntax = "proto3";
package rbk.protocol;
// SRC1100 3D 0.3dsmap
// SEER Robokit 3D 0.3dsmap
// protobuf
//