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

add configurable obstacle blocking timeout

Browse Source
This commit is contained in:
linbo 2026-08-19 14:10:02 +08:00
parent c66e25ec92
commit fad8e0e87c
8 changed files with 96 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmvr-es/common/types/agv/agv_types.h
View File

@ -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;

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

@ -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;
};

15
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h
View File

@ -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)
{

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

@ -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

60
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp
View File

@ -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;
} else {
blocked_stopped_samples = 0;
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;
}
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)
+ " 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;
} else {
blocked_stopped_samples = 0;
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;
}
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)
+ " 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

7
cmvr-es/service/grpc/action/src/action_queue_executor.cpp
View File

@ -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;
}

1
cmvr-es/service/grpc/server/src/grpc_agv_service.cpp
View File

@ -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;
}

3
protos/cmvr/api/agv_utils.proto
View File

@ -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 适配器扩展参数。用于传递厂商或控制器特有的参数。