add configurable obstacle blocking timeout

cmvr-es/common/types/agv/agv_types.h

@@ -141,6 +141,7 @@ struct AgvMotionOptions {
     bool asynchronous{false};
     int wait_timeout_ms{0};
     int poll_interval_ms{0};
+    int blocked_timeout_ms{0};
     // 不带 RPC 框架依赖的取消检查。同步导航等待期间可由
     // 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。
     std::function<bool()> cancellation_requested;

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h

@@ -124,6 +124,7 @@ private:
         bool emergency{false};
         std::string target_id;
         std::string active_faults;
+        bool only_recoverable_blocking_faults{false};
         std::string detail;
     };
 

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h

@@ -18,6 +18,8 @@ constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
 constexpr int kPoseNavigationRequiredRunningSamples = 2;
 constexpr auto kDefaultNavigationWaitTimeout =
     std::chrono::milliseconds(600000);
+constexpr auto kDefaultNavigationBlockedTimeout =
+    std::chrono::milliseconds(60000);
 constexpr auto kDefaultNavigationPollInterval =
     std::chrono::milliseconds(200);
 constexpr auto kMaximumNavigationPollInterval =
@@ -28,9 +30,9 @@ 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 kRobotBlockedFaultCode = 52200;
 constexpr int kMinimumControllerFaultCaptureGraceMs = 250;
 constexpr int kMaximumControllerFaultCaptureGraceMs = 5000;
 constexpr int kDefaultControllerFaultPushIntervalMs = 1000;
@@ -185,6 +187,9 @@ static inline std::string invalidMotionOption(const AgvMotionOptions& options)
     if (options.wait_timeout_ms < 0) {
         return "wait_timeout_ms must be non-negative";
     }
+    if (options.blocked_timeout_ms < 0) {
+        return "blocked_timeout_ms must be non-negative";
+    }
     if (options.poll_interval_ms < 0) {
         return "poll_interval_ms must be non-negative";
     }
@@ -208,6 +213,14 @@ static inline std::chrono::milliseconds navigationWaitTimeout(
         : kDefaultNavigationWaitTimeout;
 }
 
+static inline std::chrono::milliseconds navigationBlockedTimeout(
+    const AgvMotionOptions& options)
+{
+    return options.blocked_timeout_ms > 0
+        ? std::chrono::milliseconds(options.blocked_timeout_ms)
+        : kDefaultNavigationBlockedTimeout;
+}
+
 static inline std::chrono::milliseconds navigationPollInterval(
     const AgvMotionOptions& options)
 {

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

@@ -1290,7 +1290,15 @@ AgvResult SeerRobokitAgv::queryNavigationSnapshot_(
     }
 
     std::ostringstream faults;
-    const auto append_faults = [&response, &faults](const char* key) {
+    bool found_fault = false;
+    bool only_recoverable_blocking_faults = true;
+    const auto append_faults = [
+        &response,
+        &faults,
+        &found_fault,
+        &only_recoverable_blocking_faults](
+            const char* key,
+            const bool may_be_recoverable_blocking_fault) {
         const auto* value = jsonFind(response, key);
         if (!value) {
             return;
@@ -1299,6 +1307,26 @@ AgvResult SeerRobokitAgv::queryNavigationSnapshot_(
         if (!malformed && value->empty()) {
             return;
         }
+        found_fault = true;
+        bool recoverable_blocking_fault =
+            may_be_recoverable_blocking_fault && !malformed;
+        if (recoverable_blocking_fault) {
+            for (const auto& fault : *value) {
+                if (!fault.isObject()) {
+                    recoverable_blocking_fault = false;
+                    break;
+                }
+                const auto* code = jsonFind(fault, "code");
+                if (!code || !code->isNumeric()
+                    || code->asInt() != kRobotBlockedFaultCode) {
+                    recoverable_blocking_fault = false;
+                    break;
+                }
+            }
+        }
+        if (!recoverable_blocking_fault) {
+            only_recoverable_blocking_faults = false;
+        }
         if (faults.tellp() > 0) {
             faults << ", ";
         }
@@ -1310,9 +1338,11 @@ AgvResult SeerRobokitAgv::queryNavigationSnapshot_(
             faults << "(malformed; expected array)";
         }
     };
-    append_faults("fatals");
+    append_faults("fatals", false);
-    append_faults("errors");
+    append_faults("errors", true);
     snapshot.active_faults = faults.str();
+    snapshot.only_recoverable_blocking_faults =
+        found_fault && only_recoverable_blocking_faults;
 
     std::ostringstream detail;
     detail << "1101 task_status=" << snapshot.task_status

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

@@ -839,6 +839,7 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
     const auto poll_interval = navigationPollInterval(options);
     const auto deadline = std::chrono::steady_clock::now()
         + navigationWaitTimeout(options);
+    const auto blocked_timeout = navigationBlockedTimeout(options);
     const auto accepted_at = context.accepted_at
             == std::chrono::steady_clock::time_point{}
         ? std::chrono::steady_clock::now()
@@ -848,7 +849,7 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
     bool final_completion_observed = false;
     AgvErrorCode terminal_error = AgvErrorCode::OK;
     std::string terminal_reason;
-    int blocked_stopped_samples = 0;
+    std::chrono::steady_clock::time_point blocked_since;
     int terminal_stopped_samples = 0;
     std::string last_detail = "no task status received";
     bool superseded_wait_active = false;
@@ -1121,7 +1122,9 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
             && global_target_matches;
         const bool attributed_matching_global_active =
             attributed_global_active && global_matches_context;
-        if (snapshot.emergency || !snapshot.active_faults.empty()) {
+        const bool has_immediate_fault = !snapshot.active_faults.empty()
+            && !snapshot.only_recoverable_blocking_faults;
+        if (snapshot.emergency || has_immediate_fault) {
             return failAndCancelTrackedNavigation_(
                 context,
                 options,
@@ -1163,19 +1166,26 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
                       "still active and must be canceled before returning; "
                     + last_detail);
         }
-        if (snapshot.blocked && navigationStopped(snapshot)) {
+        const bool blocked_and_stopped =
-            ++blocked_stopped_samples;
+            snapshot.blocked && navigationStopped(snapshot);
-        } else {
+        const auto blocked_observed_at = std::chrono::steady_clock::now();
-            blocked_stopped_samples = 0;
+        if (blocked_and_stopped) {
+            if (blocked_since == std::chrono::steady_clock::time_point{}) {
+                blocked_since = blocked_observed_at;
             }
-        if (blocked_stopped_samples >= kRequiredBlockedStopSamples) {
+        } else {
+            blocked_since = {};
+        }
+        if (blocked_since != std::chrono::steady_clock::time_point{}
+            && blocked_observed_at - blocked_since >= blocked_timeout) {
             return failAndCancelTrackedNavigation_(
                 context,
                 options,
-                AgvErrorCode::TaskFailed,
+                AgvErrorCode::Timeout,
                 "SEER Robokit navigation remained blocked while stopped for "
-                    + std::to_string(blocked_stopped_samples)
+                    + std::to_string(blocked_timeout.count())
-                    + " consecutive 1101 samples: " + last_detail);
+                    + " ms, reaching the configured blocked timeout: "
+                    + last_detail);
         }
 
         bool terminal_candidate = terminal_error != AgvErrorCode::OK;
@@ -1255,6 +1265,7 @@ AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_(
     const auto poll_interval = navigationPollInterval(options);
     const auto deadline = std::chrono::steady_clock::now()
         + navigationWaitTimeout(options);
+    const auto blocked_timeout = navigationBlockedTimeout(options);
     const auto accepted_at = navigation_context.accepted_at
             == std::chrono::steady_clock::time_point{}
         ? std::chrono::steady_clock::now()
@@ -1264,7 +1275,7 @@ AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_(
     bool completion_observed = false;
     AgvErrorCode terminal_error = AgvErrorCode::OK;
     std::string terminal_reason;
-    int blocked_stopped_samples = 0;
+    std::chrono::steady_clock::time_point blocked_since;
     int terminal_stopped_samples = 0;
     std::string last_detail = "free-navigation task start was confirmed";
     bool superseded_wait_active = false;
@@ -1477,7 +1488,9 @@ AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_(
         const bool attributed_matching_global_active =
             attributed_global_active && snapshot.task_type == 1;
 
-        if (snapshot.emergency || !snapshot.active_faults.empty()) {
+        const bool has_immediate_fault = !snapshot.active_faults.empty()
+            && !snapshot.only_recoverable_blocking_faults;
+        if (snapshot.emergency || has_immediate_fault) {
             return failAndCancelTrackedNavigation_(
                 navigation_context,
                 options,
@@ -1521,19 +1534,26 @@ AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_(
                     + last_detail);
         }
 
-        if (snapshot.blocked && navigationStopped(snapshot)) {
+        const bool blocked_and_stopped =
-            ++blocked_stopped_samples;
+            snapshot.blocked && navigationStopped(snapshot);
-        } else {
+        const auto blocked_observed_at = std::chrono::steady_clock::now();
-            blocked_stopped_samples = 0;
+        if (blocked_and_stopped) {
+            if (blocked_since == std::chrono::steady_clock::time_point{}) {
+                blocked_since = blocked_observed_at;
             }
-        if (blocked_stopped_samples >= kRequiredBlockedStopSamples) {
+        } else {
+            blocked_since = {};
+        }
+        if (blocked_since != std::chrono::steady_clock::time_point{}
+            && blocked_observed_at - blocked_since >= blocked_timeout) {
             return failAndCancelTrackedNavigation_(
                 navigation_context,
                 options,
-                AgvErrorCode::TaskFailed,
+                AgvErrorCode::Timeout,
                 "SEER Robokit free navigation remained blocked while stopped for "
-                    + std::to_string(blocked_stopped_samples)
+                    + std::to_string(blocked_timeout.count())
-                    + " consecutive 1101 samples: " + last_detail);
+                    + " ms, reaching the configured blocked timeout: "
+                    + last_detail);
         }
 
         const bool expected_completed_snapshot = global_attribution_ready

cmvr-es/service/grpc/action/src/action_queue_executor.cpp

@@ -314,8 +314,10 @@ bool validAgvOptions(
         return false;
     }
     if (options.wait_timeout_ms() < 0 ||
-        options.poll_interval_ms() < 0) {
+        options.poll_interval_ms() < 0 ||
-        error = "AGV timeout and poll interval must be non-negative";
+        options.blocked_timeout_ms() < 0) {
+        error = "AGV wait timeout, blocked timeout, and poll interval "
+                "must be non-negative";
         return false;
     }
     if (options.poll_interval_ms() > 5000) {
@@ -386,6 +388,7 @@ device::AgvMotionOptions toAgvMotionOptions(
     destination.asynchronous = source.asynchronous();
     destination.wait_timeout_ms = source.wait_timeout_ms();
     destination.poll_interval_ms = source.poll_interval_ms();
+    destination.blocked_timeout_ms = source.blocked_timeout_ms();
     return destination;
 }
 

cmvr-es/service/grpc/server/src/grpc_agv_service.cpp

@@ -357,6 +357,7 @@ device::AgvMotionOptions toMotionOptions(
     dst.asynchronous = src.asynchronous();
     dst.wait_timeout_ms = src.wait_timeout_ms();
     dst.poll_interval_ms = src.poll_interval_ms();
+    dst.blocked_timeout_ms = src.blocked_timeout_ms();
     dst.cancellation_requested = std::move(cancellation_requested);
     return dst;
 }

protos/cmvr/api/agv_utils.proto

@@ -88,6 +88,9 @@ message AgvMotionOptions {
   int32 wait_timeout_ms = 9;
   // 同步导航的状态轮询周期，单位：毫秒；0 表示使用适配器默认值。
   int32 poll_interval_ms = 10;
+  // 连续遇障且底盘停止时的最大等待时间，单位：毫秒；
+  // 0 或未传表示使用适配器默认值 60000 毫秒。
+  int32 blocked_timeout_ms = 11;
 }
 
 // AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。