SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: implement SEER Robokit clear fault

Browse Source
This commit is contained in:
xtkuang 2026-08-18 15:04:04 +08:00
parent 8ac1bff099
commit f611ad1db8
4 changed files with 39 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cmvr-es/devices/agv/seer_robokit/README.md
View File

@ -106,7 +106,7 @@ cmake --install build
| `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` |
| `19207` | 控制权、清错、地图上传下载 | `4005`、`4009`、`4010`、`4011` |
| `19210` | 开始/停止建图 | `6100`、`6101` |
| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 |
@ -126,7 +126,7 @@ gRPC 客户端另做一套租约逻辑。
| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 |
| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 |
| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 |
| `clearFault` | 未实现 | 返回 `UnsupportedCommand` |
| `clearFault` | `4009` | 抢权后发送无请求体命令，清除可恢复故障 |
| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 |
| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 |
| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 |

1
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h
View File

@ -25,6 +25,7 @@ constexpr std::uint16_t kRobotTaskTranslate = 3055;
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
constexpr std::uint16_t kRobotConfigLock = 4005;
constexpr std::uint16_t kRobotConfigClearFault = 4009;
constexpr std::uint16_t kRobotConfigUploadMap = 4010;
constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
constexpr std::uint16_t kRobotOtherStartMapping = 6100;

10
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
View File

@ -161,9 +161,13 @@ AgvResult SeerRobokitAgv::emergencyStopTrackedNavigation_(
AgvResult SeerRobokitAgv::clearFault()
{
return AgvResult::failure(
AgvErrorCode::UnsupportedCommand,
"SEER Robokit clearFault command is not implemented");
Json::Value response;
auto result = sendControlledCommand_(
sock_config_,
kRobotConfigClearFault,
Json::Value(Json::objectValue),
&response);
return result.ok() ? resultFromResponse_(response) : result;
}
AgvResult SeerRobokitAgv::navigateToPose(

32
cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp
View File

@ -150,6 +150,7 @@ 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 kRobotConfigClearFault = 4009;
constexpr std::uint16_t kRobotConfigUploadMap = 4010;
constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
constexpr std::uint16_t kRobotOtherStartMapping = 6100;
@ -784,6 +785,9 @@ TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquire
[this]() {
return agv_->emergencyStop();
});
expectControlled(kRobotConfigClearFault, [this]() {
return agv_->clearFault();
});
expectControlled(kRobotControlMotion, [this]() {
return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2});
});
@ -3114,15 +3118,37 @@ TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggrega
EXPECT_EQ(records[2].command, kRobotTaskCancel);
}
TEST_F(SeerRobokitControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority)
TEST_F(SeerRobokitControlAuthorityTest, ClearFaultSends4009WithoutPayloadAfterAcquiringAuthority)
{
controller_.clearRecords();
const auto result = agv_->clearFault();
ASSERT_TRUE(result.ok()) << result.message;
const auto records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[0].command, kRobotConfigLock);
EXPECT_EQ(records[1].command, kRobotConfigClearFault);
EXPECT_TRUE(records[1].payload.empty());
}
TEST_F(SeerRobokitControlAuthorityTest, ClearFaultPreservesControllerFailure)
{
controller_.setResponseCode(kRobotConfigClearFault, 44009);
controller_.clearRecords();
const auto result = agv_->clearFault();
EXPECT_FALSE(result.ok());
EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand);
EXPECT_TRUE(controller_.records().empty());
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
EXPECT_NE(result.message.find("ret_code=44009"), std::string::npos);
EXPECT_NE(
result.message.find("simulated command failure"),
std::string::npos);
const auto records = controller_.records();
ASSERT_EQ(records.size(), 2U);
EXPECT_EQ(records[0].command, kRobotConfigLock);
EXPECT_EQ(records[1].command, kRobotConfigClearFault);
}
TEST_F(SeerRobokitControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority)