diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index d5e02b4d..191d7f40 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/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 cancellation_requested; diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index 5db9d086..e03ac938 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ b/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; }; diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h index 89774399..acd47b30 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h +++ b/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) { 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 66c496bd..546caedf 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 @@ -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("errors"); + append_faults("fatals", false); + 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 diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp index 308bde5d..9da80a79 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp +++ b/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)) { - ++blocked_stopped_samples; + const bool blocked_and_stopped = + snapshot.blocked && navigationStopped(snapshot); + const auto blocked_observed_at = std::chrono::steady_clock::now(); + if (blocked_and_stopped) { + if (blocked_since == std::chrono::steady_clock::time_point{}) { + blocked_since = blocked_observed_at; + } } else { - blocked_stopped_samples = 0; + blocked_since = {}; } - if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { + 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) - + " consecutive 1101 samples: " + last_detail); + + std::to_string(blocked_timeout.count()) + + " 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)) { - ++blocked_stopped_samples; + const bool blocked_and_stopped = + snapshot.blocked && navigationStopped(snapshot); + const auto blocked_observed_at = std::chrono::steady_clock::now(); + if (blocked_and_stopped) { + if (blocked_since == std::chrono::steady_clock::time_point{}) { + blocked_since = blocked_observed_at; + } } else { - blocked_stopped_samples = 0; + blocked_since = {}; } - if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { + 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) - + " consecutive 1101 samples: " + last_detail); + + std::to_string(blocked_timeout.count()) + + " ms, reaching the configured blocked timeout: " + + last_detail); } const bool expected_completed_snapshot = global_attribution_ready diff --git a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp b/cmvr-es/service/grpc/action/src/action_queue_executor.cpp index 07289f7f..d2175348 100644 --- a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp +++ b/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) { - error = "AGV timeout and poll interval must be non-negative"; + options.poll_interval_ms() < 0 || + 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; } diff --git a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp index 0c2b196d..790d72ae 100644 --- a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp +++ b/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; } diff --git a/protos/cmvr/api/agv_utils.proto b/protos/cmvr/api/agv_utils.proto index c05f1a79..39109f43 100644 --- a/protos/cmvr/api/agv_utils.proto +++ b/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 适配器扩展参数。用于传递厂商或控制器特有的参数。