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