From f611ad1db8e0730a2a18fa645f505a45ecaa5727 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 18 Aug 2026 15:04:04 +0800 Subject: [PATCH] feat: implement SEER Robokit clear fault --- cmvr-es/devices/agv/seer_robokit/README.md | 4 +-- .../include/seer_robokit_protocol.h | 1 + .../src/seer_robokit_navigation.cpp | 10 ++++-- .../seer_robokit_control_authority_test.cpp | 32 +++++++++++++++++-- 4 files changed, 39 insertions(+), 8 deletions(-) diff --git a/cmvr-es/devices/agv/seer_robokit/README.md b/cmvr-es/devices/agv/seer_robokit/README.md index b2ee3d00..3cb6d77d 100644 --- a/cmvr-es/devices/agv/seer_robokit/README.md +++ b/cmvr-es/devices/agv/seer_robokit/README.md @@ -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` 不同 | diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h index 89df8282..a0dd5db7 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h @@ -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; diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp index edf22ead..94d5137a 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp @@ -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( diff --git a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp index b1109f12..9198d90f 100644 --- a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp +++ b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp @@ -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)