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

fix(agv): handle SEER task status 404 after fast navigation

Browse Source
This commit is contained in:
linbo 2026-08-21 15:07:38 +08:00
parent fad8e0e87c
commit 67566e8aea
4 changed files with 162 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -104,10 +104,18 @@ private:
int state{0};
int type{0};
bool type_present{false};
double progress{0.0};
bool progress_present{false};
double remaining_distance{0.0};
bool distance_present{false};
std::string closest_target;
std::string source_name;
std::string target_name;
std::string detail;
};
struct NavigationSnapshot {
int task_status{0};
int task_type{0};

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

@ -1124,9 +1124,38 @@ AgvResult SeerRobokitAgv::queryTaskStatuses_(
}
const auto* package = jsonFind(response, "task_status_package");
const double progress = package
? jsonGet(*package, "percentage", 0.0).asDouble()
: 0.0;
double progress = 0.0;
bool progress_present = false;
double remaining_distance = 0.0;
bool distance_present = false;
std::string closest_target;
std::string source_name;
std::string target_name;
if (package) {
if (const auto* value = jsonFind(*package, "percentage");
value && value->isNumeric()) {
progress = value->asDouble();
progress_present = std::isfinite(progress);
}
if (const auto* value = jsonFind(*package, "distance");
value && value->isNumeric()) {
remaining_distance = value->asDouble();
distance_present = std::isfinite(remaining_distance);
}
if (const auto* value = jsonFind(*package, "closest_target");
value && value->isString()) {
closest_target = value->asString();
}
if (const auto* value = jsonFind(*package, "source_name");
value && value->isString()) {
source_name = value->asString();
}
if (const auto* value = jsonFind(*package, "target_name");
value && value->isString()) {
target_name = value->asString();
}
}
if (package) {
if (const auto* status_list = jsonFind(*package, "task_status_list");
status_list && status_list->isArray()) {
@ -1186,6 +1215,12 @@ AgvResult SeerRobokitAgv::queryTaskStatuses_(
for (std::size_t index = 0; index < requested_task_ids.size(); ++index) {
auto& status = statuses[index];
status.progress = progress;
status.progress_present = progress_present;
status.remaining_distance = remaining_distance;
status.distance_present = distance_present;
status.closest_target = closest_target;
status.source_name = source_name;
status.target_name = target_name;
std::ostringstream detail;
detail << "task_id=" << requested_task_ids[index];
if (status.found) {

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

@ -5,6 +5,8 @@
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <cstddef>
#include <cstdint>
#include <sstream>
@ -845,16 +847,55 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
? std::chrono::steady_clock::now()
: context.accepted_at;
const auto start_deadline = accepted_at + kPoseNavigationStartTimeout;
const auto fast_completion_deadline =
start_deadline + kNavigationCancelConfirmationTimeout;
bool any_task_observed = false;
bool final_completion_observed = false;
AgvErrorCode terminal_error = AgvErrorCode::OK;
std::string terminal_reason;
std::chrono::steady_clock::time_point blocked_since;
int terminal_stopped_samples = 0;
int fast_completion_stopped_samples = 0;
std::string last_detail = "no task status received";
bool superseded_wait_active = false;
std::chrono::steady_clock::time_point superseded_deadline;
// queryTaskStatuses_ formats these controller package fields into detail.
// Keep the fallback local to this waiter so older PoseTaskStatus callers do
// not acquire station-navigation-specific state.
const auto task_detail_field = [](
const std::string& detail,
const std::string& field)
-> std::string {
const std::string marker = ", " + field + "=";
const auto value_begin = detail.find(marker);
if (value_begin == std::string::npos) {
return {};
}
const auto begin = value_begin + marker.size();
const auto end = detail.find(", ", begin);
return detail.substr(
begin,
end == std::string::npos ? std::string::npos : end - begin);
};
const auto task_detail_number = [&task_detail_field](
const std::string& detail,
const std::string& field,
double& value) {
const std::string text = task_detail_field(detail, field);
if (text.empty()) {
return false;
}
char* end = nullptr;
const double parsed = std::strtod(text.c_str(), &end);
if (end == text.c_str() || *end != '\0' || !std::isfinite(parsed)) {
return false;
}
value = parsed;
return true;
};
while (std::chrono::steady_clock::now() < deadline) {
TrackedNavigationContext active_context;
const bool still_current = currentTrackedNavigation_(active_context)
@ -959,8 +1000,35 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
currentTrackedNavigation_(post_query_context)
&& post_query_context.token == context.token;
if (!any_task_observed
&& std::chrono::steady_clock::now() >= start_deadline) {
const auto establishment_observed_at =
std::chrono::steady_clock::now();
const bool task_start_timed_out = !any_task_observed
&& establishment_observed_at >= start_deadline;
const PoseTaskStatus* missing_exact_status =
task_statuses.size() == 1 ? &task_statuses.front() : nullptr;
double fast_completion_distance = 0.0;
const bool fast_completion_distance_present = missing_exact_status
&& task_detail_number(
missing_exact_status->detail,
"distance",
fast_completion_distance);
const bool fast_completion_hint = task_start_timed_out
&& context.type == AgvTaskType::NavigateToStation
&& missing_exact_status
&& missing_exact_status->found
&& missing_exact_status->state == 404
&& std::isfinite(missing_exact_status->progress)
&& missing_exact_status->progress >= 1.0
&& fast_completion_distance_present
&& std::abs(fast_completion_distance) <= 0.01
&& task_detail_field(
missing_exact_status->detail,
"target_name") == context.target_id
&& task_detail_field(
missing_exact_status->detail,
"closest_target") == context.target_id;
if (task_start_timed_out && !fast_completion_hint) {
if (!still_current_after_query) {
return AgvResult::failure(
AgvErrorCode::CommandFailed,
@ -1166,6 +1234,44 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
"still active and must be canceled before returning; "
+ last_detail);
}
if (fast_completion_hint) {
const bool terminal_snapshot = snapshot.task_status == 0
|| (snapshot.task_status == 4
&& global_type_matches
&& (snapshot.target_id.empty()
|| snapshot.target_id == context.target_id));
const bool safe_and_stopped = terminal_snapshot
&& !snapshot.blocked
&& snapshot.active_faults.empty()
&& navigationStopped(snapshot);
if (safe_and_stopped) {
++fast_completion_stopped_samples;
} else {
fast_completion_stopped_samples = 0;
}
if (fast_completion_stopped_samples
>= kRequiredCompletedStopSamples) {
clearTrackedNavigationIfToken_(context.token);
return AgvResult::success();
}
if (std::chrono::steady_clock::now()
>= fast_completion_deadline) {
return failAndCancelTrackedNavigation_(
context,
options,
AgvErrorCode::TaskRejected,
"SEER Robokit navigation task id remained unavailable and "
"fast-completion verification did not establish a matching "
"terminal stopped state: " + last_detail);
}
sleepForNavigationPoll(
poll_interval,
std::min(deadline, fast_completion_deadline),
options);
continue;
}
const bool blocked_and_stopped =
snapshot.blocked && navigationStopped(snapshot);
const auto blocked_observed_at = std::chrono::steady_clock::now();