diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 8abe3372..042bdc0c 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -40,14 +40,14 @@ public: * Action execution. */ virtual bool supportsSynchronousAction(AgvActionKind) const noexcept - { - return false; - } + { + return false; + } - /** - * @brief 获取 AGV 运行状态快照。 - */ - virtual AgvRuntimeState runtimeState() const { return {}; } + /** + * @brief 获取 AGV 运行状态快照。 + */ + virtual AgvRuntimeState runtimeState() const { return {}; } /** * @brief 获取当前导航任务状态。 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 e03ac938..6a61abd4 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 @@ -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}; 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 546caedf..57ca85ef 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 @@ -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) { 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 9da80a79..5e8dbaab 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 @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include #include @@ -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();