feat: implement SEER Robokit clear fault

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` 不同 |

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;

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp

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

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_EQ(result.code, AgvErrorCode::CommandFailed);
-    EXPECT_TRUE(controller_.records().empty());
+    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)