From 26a7ad5d4b89058f1a9225e9dc008e21c61f5d29 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 17:50:49 +0800 Subject: [PATCH] fix: harden SRC1100 free navigation handling --- .../devices/agv/src1100/include/src1100_agv.h | 61 +- .../devices/agv/src1100/src/src1100_agv.cpp | 1464 ++++++++++++++-- .../tests/src1100_control_authority_test.cpp | 1491 ++++++++++++++++- 3 files changed, 2838 insertions(+), 178 deletions(-) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 3aa08913..e9fa3a82 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -2,6 +2,7 @@ #define CMVR_ES_SRC1100_AGV_H #include +#include #include #include #include @@ -77,6 +78,25 @@ private: int push{19301}; }; + struct PoseTaskStatus { + bool found{false}; + int state{0}; + int type{0}; + double progress{0.0}; + std::string detail; + }; + + struct PoseTaskContext { + std::string task_id; + math::Pose2d target{}; + double reach_distance{0.0}; + double reach_angle{0.0}; + std::uint64_t navigation_generation{0}; + std::uint64_t controller_fault_sequence_at_start{0}; + std::uint64_t control_attempt_sequence_at_start{0}; + std::uint64_t controller_fault_channel_epoch_at_start{0}; + }; + AgvResult connect_(); AgvResult disconnect_(); AgvResult connectSocket_(int& sock, int port); @@ -86,13 +106,37 @@ private: AgvResult acquireControl_() const; AgvResult confirmPoseNavigationStarted_( + const PoseTaskContext& context) const; + AgvResult queryPoseTaskStatus_( const std::string& task_id, - std::uint64_t navigation_generation) const; + PoseTaskStatus& status) const; + bool poseTargetReached_( + const PoseTaskContext& context, + std::string& detail) const; + std::string cachedControllerFaultDetail_( + std::uint64_t after_sequence = 0, + int wait_ms = 0, + std::uint64_t* associated_control_attempt = nullptr) const; + int controllerFaultCaptureGraceMs_() const; + int controllerFaultStateMaxAgeMs_() const; + std::string freeNavigationFaultStateUnavailableDetail_() const; + void rememberPoseTask_(const PoseTaskContext& context) const; + void advancePoseTaskGeneration_( + std::uint64_t navigation_generation, + std::uint64_t control_attempt_sequence) const; + void advancePoseTaskControlAttempt_( + std::uint64_t control_attempt_sequence) const; + void clearPoseTask_(std::uint64_t navigation_generation) const; + bool currentPoseTask_(PoseTaskContext& context) const; AgvResult sendControlledCommand_(int sock, std::uint16_t command, const Json::Value& payload, Json::Value* response, - std::uint64_t* accepted_navigation_generation = nullptr) const; + std::uint64_t* accepted_navigation_generation = nullptr, + std::uint64_t* controller_fault_sequence_at_attempt = nullptr, + std::uint64_t* control_attempt_sequence = nullptr, + PoseTaskContext* pose_context_to_publish = nullptr, + bool reject_if_active_controller_fault = false) const; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -106,6 +150,7 @@ private: void startPushThread_(); void stopPushThread_(); void pushLoop_(); + void invalidateControllerFaultState_(); AgvRuntimeState queryRuntimeState_() const; void updateCachedRuntimeState_(const Json::Value& payload); void startMapUpdateThread_(); @@ -168,6 +213,10 @@ private: mutable std::mutex control_sequence_mutex_; mutable std::atomic navigation_generation_{0}; mutable std::atomic pose_task_sequence_{0}; + mutable std::atomic control_attempt_sequence_{0}; + mutable std::atomic controller_fault_channel_epoch_{0}; + mutable std::mutex pose_task_mutex_; + mutable PoseTaskContext pose_task_context_; mutable int sock_status_{-1}; mutable int sock_control_{-1}; mutable int sock_navigation_{-1}; @@ -179,8 +228,16 @@ private: std::atomic push_running_{false}; std::thread push_thread_; mutable std::mutex runtime_state_mutex_; + mutable std::condition_variable runtime_state_cv_; AgvRuntimeState cached_runtime_state_; bool cached_runtime_state_valid_{false}; + std::uint64_t controller_fault_sequence_{0}; + bool controller_fault_state_observed_{false}; + std::chrono::steady_clock::time_point controller_fault_state_observed_at_{}; + std::string active_controller_fault_detail_; + double last_controller_fault_timestamp_{0.0}; + std::string last_controller_fault_detail_; + std::uint64_t last_controller_fault_control_attempt_{0}; mutable std::atomic map_update_running_{false}; mutable std::thread map_update_thread_; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 6c9f5ff8..d523c80d 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -54,6 +54,16 @@ constexpr std::uint16_t kRobotPush = 19301; constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U; constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500); constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50); +constexpr int kPoseNavigationRequiredRunningSamples = 2; +constexpr int kMinimumControllerFaultCaptureGraceMs = 250; +constexpr int kMaximumControllerFaultCaptureGraceMs = 5000; +constexpr int kDefaultControllerFaultPushIntervalMs = 1000; +constexpr int kControllerFaultPushJitterMs = 100; +constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000; +constexpr int kControllerFaultStateMaxAgeIntervals = 5; +constexpr double kDefaultPoseReachDistance = 0.05; +constexpr double kDefaultPoseReachAngle = 0.10; +constexpr double kTwoPi = 6.28318530717958647692; constexpr int kDefaultMapUpdateIntervalMs = 1000; constexpr std::size_t kDefaultMapUpdateHistorySize = 8; constexpr std::uint64_t kMapSnapshotSequenceStart = 1; @@ -168,6 +178,11 @@ double nowSeconds() return std::chrono::duration(now).count(); } +double angleDistance(const double lhs, const double rhs) +{ + return std::abs(std::remainder(lhs - rhs, kTwoPi)); +} + bool jsonHas(const Json::Value& value, const char* key) { return jsonFind(value, key) != nullptr; @@ -370,6 +385,54 @@ AgvTaskType toTaskType(const int value) } } +std::string invalidMotionOption(const AgvMotionOptions& options) +{ + const auto non_negative_error = [](const double value, const char* field) { + if (!std::isfinite(value)) { + return std::string(field) + " must be finite"; + } + if (value < 0.0) { + return std::string(field) + " must be non-negative"; + } + return std::string{}; + }; + + if (auto error = non_negative_error(options.max_speed, "max_speed"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_angular_speed, + "max_angular_speed"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_acceleration, + "max_acceleration"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_angular_acceleration, + "max_angular_acceleration"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.reach_distance, + "reach_distance"); + !error.empty()) return error; + if (auto error = non_negative_error(options.reach_angle, "reach_angle"); + !error.empty()) return error; + if (auto error = non_negative_error(options.speed_ratio, "speed_ratio"); + !error.empty()) return error; + return {}; +} + +bool parseFiniteDouble(const std::string& value, double& parsed) +{ + std::size_t consumed = 0; + try { + parsed = std::stod(value, &consumed); + } catch (...) { + return false; + } + return consumed == value.size() && std::isfinite(parsed); +} + } // namespace Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) @@ -429,28 +492,48 @@ bool Src1100Agv::update() AgvRuntimeState Src1100Agv::runtimeState() const { + AgvRuntimeState cached_state; + bool has_cached_state = false; if (state_push_enabled_) { std::lock_guard lock(runtime_state_mutex_); if (cached_runtime_state_valid_) { - auto state = cached_runtime_state_; - state.connected = connected_(); - state.last_error = last_error_; - if (!state.connected) { - state.mode = AgvMode::Disconnected; - } - return state; + cached_state = cached_runtime_state_; + has_cached_state = true; } } + if (has_cached_state) { + std::string adapter_error; + { + std::lock_guard lock(mutex_); + cached_state.connected = connected_(); + adapter_error = last_error_; + } + if (!adapter_error.empty()) { + if (cached_state.last_error.empty()) { + cached_state.last_error = adapter_error; + } else if (cached_state.last_error != adapter_error) { + cached_state.last_error += "; adapter_error=" + adapter_error; + } + } + if (!cached_state.connected) { + cached_state.mode = AgvMode::Disconnected; + } + return cached_state; + } + return queryRuntimeState_(); } AgvRuntimeState Src1100Agv::queryRuntimeState_() const { AgvRuntimeState state; - state.connected = connected_(); + { + std::lock_guard lock(mutex_); + state.connected = connected_(); + state.last_error = last_error_; + } state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; - state.last_error = last_error_; Json::Value loc; if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { @@ -485,6 +568,248 @@ AgvRuntimeState Src1100Agv::queryRuntimeState_() const AgvNavigationStatus Src1100Agv::navigationStatus() const { AgvNavigationStatus status; + std::string missing_pose_task_detail; + for (int attempt = 0; attempt < 2; ++attempt) { + PoseTaskContext pose_context; + if (!currentPoseTask_(pose_context)) { + break; + } + const auto observed_navigation_generation = + navigation_generation_.load(std::memory_order_relaxed); + if (pose_context.navigation_generation + != observed_navigation_generation) { + continue; + } + + PoseTaskStatus task_status; + const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status); + PoseTaskContext latest_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(latest_context) + || latest_context.navigation_generation + != pose_context.navigation_generation + || latest_context.task_id != pose_context.task_id) { + continue; + } + + status.type = AgvTaskType::NavigateToPose; + const auto fault_monitoring_unavailable = + [this, &pose_context]() { + if (controller_fault_channel_epoch_.load( + std::memory_order_relaxed) + != pose_context + .controller_fault_channel_epoch_at_start) { + return std::string( + "the controller fault push channel changed or was " + "invalidated after the free-navigation command was " + "accepted"); + } + return freeNavigationFaultStateUnavailableDetail_(); + }; + if (!result.ok()) { + status.state = AgvTaskState::Failed; + status.message = result.message; + return status; + } + if (!task_status.found) { + std::uint64_t missing_task_fault_control_attempt = 0; + const std::string missing_task_fault = + cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &missing_task_fault_control_attempt); + PoseTaskContext post_missing_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_missing_context) + || post_missing_context.navigation_generation + != pose_context.navigation_generation + || post_missing_context.task_id != pose_context.task_id) { + continue; + } + if (!missing_task_fault.empty()) { + std::string attribution; + if (missing_task_fault_control_attempt != 0 + && missing_task_fault_control_attempt + != pose_context.control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 tracked free-navigation task disappeared from " + "1110 task_status_package while a new controller fault " + "was observed: " + task_status.detail + ", " + + attribution + missing_task_fault; + clearPoseTask_(pose_context.navigation_generation); + return status; + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 tracked free-navigation status is unsafe to " + "accept because controller fault monitoring is " + "unavailable: " + unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } + missing_pose_task_detail = task_status.detail; + clearPoseTask_(pose_context.navigation_generation); + break; + } + if (task_status.type != 1) { + status.state = AgvTaskState::Failed; + status.type = toTaskType(task_status.type); + status.message = + "SRC1100 returned an unexpected task type for the tracked " + "free-navigation task: " + task_status.detail; + clearPoseTask_(pose_context.navigation_generation); + return status; + } + status.state = toTaskState(task_status.state); + status.progress = task_status.progress; + status.message = task_status.detail; + const auto controller_reported_state = status.state; + const bool controller_state_terminal = + controller_reported_state == AgvTaskState::Completed + || controller_reported_state == AgvTaskState::Failed + || controller_reported_state == AgvTaskState::Canceled; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + controller_reported_state == AgvTaskState::Completed + || controller_reported_state == AgvTaskState::Failed + ? controllerFaultCaptureGraceMs_() + : 0, + &fault_control_attempt); + PoseTaskContext post_fault_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_fault_context) + || post_fault_context.navigation_generation + != pose_context.navigation_generation + || post_fault_context.task_id != pose_context.task_id) { + continue; + } + const std::string unavailable = + fault_monitoring_unavailable(); + if (!fault.empty()) { + std::string attribution; + if (fault_control_attempt != 0 + && fault_control_attempt + != pose_context.control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 reported a new controller fault while the tracked " + "free-navigation task had controller_task_state=" + + std::to_string(task_status.state) + ": " + + task_status.detail + ", " + attribution + fault; + if (!unavailable.empty()) { + status.message += + ", controller_fault_monitoring_unavailable=" + + unavailable; + } + } else if (!unavailable.empty()) { + if (controller_reported_state == AgvTaskState::Failed + || controller_reported_state == AgvTaskState::Canceled) { + status.message += + ", controller_fault_monitoring_unavailable=" + + unavailable; + } else { + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 tracked free-navigation state is unsafe to accept " + "because controller fault monitoring became unavailable: " + + unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } + } + if (status.state == AgvTaskState::Completed) { + std::string pose_detail; + const bool target_reached = + poseTargetReached_(pose_context, pose_detail); + PoseTaskContext post_pose_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_pose_context) + || post_pose_context.navigation_generation + != pose_context.navigation_generation + || post_pose_context.task_id != pose_context.task_id) { + continue; + } + std::uint64_t post_pose_fault_control_attempt = 0; + const std::string post_pose_fault = + cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + 0, + &post_pose_fault_control_attempt); + if (!post_pose_fault.empty()) { + status.state = AgvTaskState::Failed; + std::string attribution; + if (post_pose_fault_control_attempt != 0 + && post_pose_fault_control_attempt + != pose_context + .control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.message = + "SRC1100 reported the tracked free-navigation task " + "Completed, but a new controller fault was observed during " + "target verification: " + task_status.detail + ", " + + attribution + post_pose_fault; + } else if (const std::string post_pose_unavailable = + fault_monitoring_unavailable(); + !post_pose_unavailable.empty()) { + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 tracked free-navigation completion is unsafe to " + "accept because controller fault monitoring became " + "unavailable: " + post_pose_unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } else if (!target_reached) { + status.state = AgvTaskState::Failed; + status.message = + "SRC1100 reported the tracked free-navigation task " + "Completed, but the requested target was not reached: " + + task_status.detail + ", " + pose_detail; + } else { + status.message += ", target_verified: " + pose_detail; + } + } + if (controller_state_terminal) { + clearPoseTask_(pose_context.navigation_generation); + } + return status; + } + + PoseTaskContext changed_context; + if (currentPoseTask_(changed_context)) { + status.state = AgvTaskState::Waiting; + status.type = AgvTaskType::NavigateToPose; + status.message = + "SRC1100 free-navigation task changed while its status was being " + "queried; query navigation status again"; + return status; + } + Json::Value payload(Json::objectValue); jsonMember(payload, "simple") = false; @@ -492,19 +817,33 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); if (!result.ok()) { status.state = AgvTaskState::Failed; - status.message = result.message; + status.message = missing_pose_task_detail.empty() + ? result.message + : missing_pose_task_detail + "; 1020 status query failed: " + + result.message; return status; } const auto controller_result = resultFromResponse_(response); if (!controller_result.ok()) { status.state = AgvTaskState::Failed; - status.message = controller_result.message; + status.message = missing_pose_task_detail.empty() + ? controller_result.message + : missing_pose_task_detail + "; 1020 status query failed: " + + controller_result.message; return status; } status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); + if (!missing_pose_task_detail.empty()) { + status.message = missing_pose_task_detail + + "; fallback_1020_status=" + std::to_string( + jsonGet(response, "task_status", 0).asInt()) + + ", fallback_1020_type=" + std::to_string( + jsonGet(response, "task_type", 0).asInt()) + + (status.message.empty() ? std::string{} : ", " + status.message); + } if (const auto* task_status_package = jsonFind(response, "task_status_package")) { status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); } @@ -513,6 +852,9 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const AgvResult Src1100Agv::connect_() { + const auto lifecycle_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + clearPoseTask_(lifecycle_generation); stopPushThread_(); stopMapUpdateThread_(); @@ -592,6 +934,9 @@ AgvResult Src1100Agv::connect_() AgvResult Src1100Agv::disconnect_() { + const auto lifecycle_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + clearPoseTask_(lifecycle_generation); stopMapUpdateThread_(); stopPushThread_(); std::lock_guard status_io_lock(status_io_mutex_); @@ -612,6 +957,10 @@ AgvResult Src1100Agv::emergencyStop() // authority acquisition so no other command from this process can // interleave between them. std::lock_guard sequence_lock(control_sequence_mutex_); + control_attempt_sequence_.fetch_add( + 1, + std::memory_order_relaxed); + const auto authority = acquireControl_(); if (!authority.ok()) { const std::string detail = authority.message.empty() ? "unknown error" : authority.message; @@ -661,6 +1010,10 @@ AgvResult Src1100Agv::emergencyStop() advance_generation_if_needed(motion_stop); const auto navigation_cancel = send_stop(sock_navigation_, kRobotTaskCancel); advance_generation_if_needed(navigation_cancel); + if (generation_advanced) { + clearPoseTask_( + navigation_generation_.load(std::memory_order_relaxed)); + } if (!motion_stop.result.ok()) { const std::string detail = motion_stop.result.message.empty() @@ -702,10 +1055,24 @@ AgvResult Src1100Agv::navigateToPose( const AgvMotionOptions& options, const AgvAdapterParams& adapter_params) { + if (!std::isfinite(pose.x) + || !std::isfinite(pose.y) + || !std::isfinite(pose.theta)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 free-navigation pose x, y, and theta must be finite"); + } + if (const std::string error = invalidMotionOption(options); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 free-navigation motion option " + error); + } + // This SRC1100 firmware exposes arbitrary-pose navigation through the - // vendor-specific freeGo extension of API 3051. Keep the required station - // identifiers non-empty even though the controller ignores id when freeGo - // is present. + // vendor-specific freeGo extension of API 3051. The empty target id and + // GotoSpecifiedPose skill are part of the controller payload that was + // validated on the differential-drive chassis. std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION"); if (source_id.empty()) { source_id = "SELF_POSITION"; @@ -715,19 +1082,23 @@ AgvResult Src1100Agv::navigateToPose( AgvErrorCode::InvalidArgument, "SRC1100 free navigation source_id must be SELF_POSITION"); } - std::string target_id = adapter_params.getString("target_id").value_or("SELF_POSITION"); - if (target_id.empty()) { - target_id = "SELF_POSITION"; - } - if (target_id != "SELF_POSITION") { + const std::string requested_target_id = + adapter_params.getString("target_id").value_or(""); + if (!requested_target_id.empty() + && requested_target_id != "SELF_POSITION") { return AgvResult::failure( AgvErrorCode::InvalidArgument, - "SRC1100 free navigation target_id must be SELF_POSITION so a " + "SRC1100 free navigation target_id must be empty or SELF_POSITION so a " "malformed freeGo request cannot fall back to station navigation"); } + const std::string target_id; - const auto skill_name = adapter_params.getString("skill_name"); - if (skill_name && !skill_name->empty() && *skill_name != "GotoSpecifiedPose") { + std::string skill_name = + adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); + if (skill_name.empty()) { + skill_name = "GotoSpecifiedPose"; + } + if (skill_name != "GotoSpecifiedPose") { return AgvResult::failure( AgvErrorCode::InvalidArgument, "SRC1100 free navigation skill_name must be GotoSpecifiedPose"); @@ -735,17 +1106,19 @@ AgvResult Src1100Agv::navigateToPose( const auto task_sequence = pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - const std::string task_id_prefix = + std::string task_id_prefix = adapter_params.getString("task_id").value_or(id_); - const std::string task_id = makePoseTaskId(task_id_prefix, task_sequence); + if (task_id_prefix.empty()) { + task_id_prefix = id_; + } + const std::string task_id = + makePoseTaskId(task_id_prefix, task_sequence); Json::Value payload(Json::objectValue); jsonMember(payload, "source_id") = source_id; jsonMember(payload, "id") = target_id; jsonMember(payload, "task_id") = task_id; - if (skill_name && !skill_name->empty()) { - jsonMember(payload, "skill_name") = *skill_name; - } + jsonMember(payload, "skill_name") = skill_name; auto& free_go = jsonMember(payload, "freeGo"); jsonMember(free_go, "x") = pose.x; @@ -757,6 +1130,16 @@ AgvResult Src1100Agv::navigateToPose( // operations such as lift, fork, script, or digital-I/O actions. applyMotionOptions_(payload, options); + PoseTaskContext context; + context.task_id = task_id; + context.target = pose; + context.reach_distance = options.reach_distance > 0.0 + ? options.reach_distance + : kDefaultPoseReachDistance; + context.reach_angle = options.reach_angle > 0.0 + ? options.reach_angle + : kDefaultPoseReachAngle; + Json::Value response; std::uint64_t navigation_generation = 0; auto result = sendControlledCommand_( @@ -764,11 +1147,15 @@ AgvResult Src1100Agv::navigateToPose( kRobotTaskGoTarget, payload, &response, - &navigation_generation); + &navigation_generation, + nullptr, + nullptr, + &context, + true); if (!result.ok()) { return result; } - return confirmPoseNavigationStarted_(task_id, navigation_generation); + return confirmPoseNavigationStarted_(context); } AgvResult Src1100Agv::navigateToStation( @@ -776,6 +1163,22 @@ AgvResult Src1100Agv::navigateToStation( const AgvMotionOptions& options, const AgvAdapterParams& adapter_params) { + if (const std::string error = invalidMotionOption(options); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 station-navigation motion option " + error); + } + if (const auto jack_height = adapter_params.getString("jack_height")) { + double parsed_jack_height = 0.0; + if (!parseFiniteDouble(*jack_height, parsed_jack_height)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 station-navigation adapter jack_height must be a " + "complete finite number"); + } + } + Json::Value payload(Json::objectValue); jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); jsonMember(payload, "id") = station_id; @@ -785,12 +1188,16 @@ AgvResult Src1100Agv::navigateToStation( applyMotionOptions_(payload, options); Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskGoTarget, payload, &response, &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::followPath(const std::vector& path) @@ -808,64 +1215,121 @@ AgvResult Src1100Agv::followPath(const std::vector& path) jsonMember(payload, "move_task_list") = tasks; Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskGoTargetList, payload, &response, &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::pauseNavigation() { Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response, - &accepted_generation); + &accepted_generation, + nullptr, + &control_attempt_sequence); + if (accepted_generation != 0) { + advancePoseTaskGeneration_( + accepted_generation, + control_attempt_sequence); + } + return result; } AgvResult Src1100Agv::resumeNavigation() { Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response, - &accepted_generation); + &accepted_generation, + nullptr, + &control_attempt_sequence); + if (accepted_generation != 0) { + advancePoseTaskGeneration_( + accepted_generation, + control_attempt_sequence); + } + return result; } AgvResult Src1100Agv::cancelNavigation() { Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response, &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) { + if (!std::isfinite(velocity.vx) + || !std::isfinite(velocity.vy) + || !std::isfinite(velocity.wz)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 velocity vx, vy, and wz must be finite"); + } + Json::Value payload(Json::objectValue); jsonMember(payload, "vx") = velocity.vx; jsonMember(payload, "vy") = velocity.vy; jsonMember(payload, "w") = velocity.wz; Json::Value response; + const bool stop_velocity = + velocity.vx == 0.0 && velocity.vy == 0.0 && velocity.wz == 0.0; + if (stop_velocity) { + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlMotion, + payload, + &response, + nullptr, + nullptr, + &control_attempt_sequence); + result = result.ok() ? resultFromResponse_(response) : result; + if (result.ok()) { + advancePoseTaskControlAttempt_(control_attempt_sequence); + } + return result; + } + std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + auto result = sendControlledCommand_( sock_control_, kRobotControlMotion, payload, &response, &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::listMaps(std::vector& maps) const @@ -908,8 +1372,17 @@ AgvResult Src1100Agv::switchMap(const std::string& map_name) Json::Value payload(Json::objectValue); jsonMember(payload, "map_name") = map_name; Json::Value response; - auto result = sendControlledCommand_(sock_control_, kRobotControlLoadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlLoadMap, + payload, + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& content) @@ -946,8 +1419,16 @@ AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) } Json::Value response; - result = sendControlledCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); - result = result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + result = sendControlledCommand_( + sock_other_, + kRobotOtherStartMapping, + payload, + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } if (result.ok()) { { std::lock_guard lock(map_update_mutex_); @@ -1658,12 +2139,17 @@ AgvResult Src1100Agv::stopMapping() if (!result.ok()) return result; Json::Value response; + std::uint64_t accepted_generation = 0; result = sendControlledCommand_( sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), - &response); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::connectSocket_(int& sock, const int port) @@ -1728,42 +2214,399 @@ AgvResult Src1100Agv::acquireControl_() const return result.ok() ? resultFromResponse_(response) : result; } -AgvResult Src1100Agv::confirmPoseNavigationStarted_( - const std::string& task_id, +void Src1100Agv::rememberPoseTask_(const PoseTaskContext& context) const +{ + std::lock_guard lock(pose_task_mutex_); + if (context.navigation_generation + < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_ = context; +} + +void Src1100Agv::advancePoseTaskGeneration_( + const std::uint64_t navigation_generation, + const std::uint64_t control_attempt_sequence) const +{ + std::lock_guard lock(pose_task_mutex_); + if (navigation_generation + < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_.navigation_generation = navigation_generation; + pose_task_context_.control_attempt_sequence_at_start = + control_attempt_sequence; +} + +void Src1100Agv::advancePoseTaskControlAttempt_( + const std::uint64_t control_attempt_sequence) const +{ + std::lock_guard lock(pose_task_mutex_); + if (pose_task_context_.task_id.empty() + || control_attempt_sequence + < pose_task_context_.control_attempt_sequence_at_start) { + return; + } + pose_task_context_.control_attempt_sequence_at_start = + control_attempt_sequence; +} + +void Src1100Agv::clearPoseTask_( const std::uint64_t navigation_generation) const +{ + std::lock_guard lock(pose_task_mutex_); + if (navigation_generation < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_ = PoseTaskContext{}; + pose_task_context_.navigation_generation = navigation_generation; +} + +bool Src1100Agv::currentPoseTask_(PoseTaskContext& context) const +{ + std::lock_guard lock(pose_task_mutex_); + if (pose_task_context_.task_id.empty()) { + return false; + } + context = pose_task_context_; + return true; +} + +std::string Src1100Agv::cachedControllerFaultDetail_( + const std::uint64_t after_sequence, + const int wait_ms, + std::uint64_t* associated_control_attempt) const +{ + std::unique_lock lock(runtime_state_mutex_); + const auto has_matching_fault = [this, after_sequence]() { + return !last_controller_fault_detail_.empty() + && controller_fault_sequence_ > after_sequence; + }; + if (!has_matching_fault() + && wait_ms > 0 + && state_push_enabled_) { + runtime_state_cv_.wait_for( + lock, + std::chrono::milliseconds(wait_ms), + has_matching_fault); + } + if (!has_matching_fault()) { + return {}; + } + if (associated_control_attempt) { + *associated_control_attempt = + last_controller_fault_control_attempt_; + } + return "cached_controller_fault_at=" + + std::to_string(last_controller_fault_timestamp_) + + ", " + last_controller_fault_detail_; +} + +int Src1100Agv::controllerFaultCaptureGraceMs_() const +{ + const int configured_interval = config_.state_push_interval_ms(); + const int effective_interval = configured_interval > 0 + ? configured_interval + : kDefaultControllerFaultPushIntervalMs; + const auto configured_grace = + static_cast(effective_interval) + + kControllerFaultPushJitterMs; + return static_cast(std::min( + std::max( + configured_grace, + static_cast(kMinimumControllerFaultCaptureGraceMs)), + static_cast(kMaximumControllerFaultCaptureGraceMs))); +} + +int Src1100Agv::controllerFaultStateMaxAgeMs_() const +{ + const int configured_interval = config_.state_push_interval_ms(); + const int effective_interval = configured_interval > 0 + ? std::min( + configured_interval, + kMaximumControllerFaultCaptureGraceMs + - kControllerFaultPushJitterMs) + : kDefaultControllerFaultPushIntervalMs; + const auto max_age = + static_cast(effective_interval) + * kControllerFaultStateMaxAgeIntervals + + kControllerFaultPushJitterMs; + return static_cast(std::max( + max_age, + static_cast(kMinimumControllerFaultStateMaxAgeMs))); +} + +std::string Src1100Agv::freeNavigationFaultStateUnavailableDetail_() const +{ + std::lock_guard lock(runtime_state_mutex_); + if (!state_push_enabled_) { + return "controller fault state is unavailable because state push is " + "disabled"; + } + // A newly reported active fault is handled through the sequenced fault + // cache, including its raw fatals/errors payload. Do not replace that + // diagnostic with the less specific "incomplete push" message. + if (!active_controller_fault_detail_.empty()) { + return {}; + } + if (!controller_fault_state_observed_) { + return "no complete state push containing fatals/errors is currently " + "available"; + } + const auto fault_state_age = + std::chrono::duration_cast( + std::chrono::steady_clock::now() + - controller_fault_state_observed_at_) + .count(); + const int max_age_ms = controllerFaultStateMaxAgeMs_(); + if (fault_state_age > max_age_ms) { + return "the most recent fatals/errors state push is stale (age_ms=" + + std::to_string(fault_state_age) + + ", max_age_ms=" + std::to_string(max_age_ms) + ")"; + } + return {}; +} + +AgvResult Src1100Agv::queryPoseTaskStatus_( + const std::string& task_id, + PoseTaskStatus& status) const +{ + status = PoseTaskStatus{}; + + Json::Value payload(Json::objectValue); + Json::Value task_ids(Json::arrayValue); + task_ids.append(task_id); + jsonMember(payload, "task_ids") = std::move(task_ids); + + Json::Value response; + auto result = sendCommand_( + sock_status_, + kRobotStatusTaskPackage, + payload, + &response); + if (!result.ok()) { + return result; + } + result = resultFromResponse_(response); + if (!result.ok()) { + return result; + } + + const auto* package = jsonFind(response, "task_status_package"); + if (package) { + status.progress = jsonGet(*package, "percentage", 0.0).asDouble(); + if (const auto* status_list = jsonFind(*package, "task_status_list"); + status_list && status_list->isArray()) { + for (const auto& item : *status_list) { + if (jsonGet(item, "task_id", "").asString() != task_id) { + continue; + } + status.found = true; + status.state = jsonGet(item, "status", 0).asInt(); + status.type = jsonGet(item, "type", 0).asInt(); + break; + } + } + } + + std::ostringstream detail; + detail << "task_id=" << task_id; + if (status.found) { + detail << ", task_status=" << status.state + << ", task_type=" << status.type; + } else { + detail << " not present in task_status_package"; + } + if (const auto* ret_code = jsonFind(response, "ret_code")) { + detail << ", status_query_ret_code=" << jsonValueToString(*ret_code); + } + const auto append_field = [&detail]( + const Json::Value& object, + const char* key, + const char* label) { + const auto* value = jsonFind(object, key); + if (!value || value->isNull()) { + return; + } + const std::string text = jsonValueToString(*value); + if (text.empty()) { + return; + } + detail << ", " << label << "=" << text; + }; + if (package) { + append_field(*package, "info", "info"); + append_field(*package, "closest_target", "closest_target"); + append_field(*package, "source_name", "source_name"); + append_field(*package, "target_name", "target_name"); + append_field(*package, "percentage", "percentage"); + append_field(*package, "distance", "distance"); + } + append_field(response, "create_on", "create_on"); + append_field(response, "err_msg", "status_query_err_msg"); + status.detail = detail.str(); + return AgvResult::success(); +} + +bool Src1100Agv::poseTargetReached_( + const PoseTaskContext& context, + std::string& detail) const +{ + math::Pose2d current_pose; + bool current_pose_available = false; + std::string pose_source; + std::string query_error; + + Json::Value response; + auto result = sendCommand_( + sock_status_, + kRobotStatusLoc, + Json::Value(Json::objectValue), + &response); + if (result.ok()) { + result = resultFromResponse_(response); + } + const auto* x = jsonFind(response, "x"); + const auto* y = jsonFind(response, "y"); + const auto* angle = jsonFind(response, "angle"); + if (result.ok() + && x && x->isNumeric() + && y && y->isNumeric() + && angle && angle->isNumeric()) { + current_pose.x = x->asDouble(); + current_pose.y = y->asDouble(); + current_pose.theta = angle->asDouble(); + if (std::isfinite(current_pose.x) + && std::isfinite(current_pose.y) + && std::isfinite(current_pose.theta)) { + current_pose_available = true; + pose_source = "controller_1004"; + } else { + query_error = "SRC1100 1004 response contained non-finite x/y/angle"; + } + } else if (!result.ok()) { + query_error = result.message; + } else { + query_error = + "SRC1100 1004 response did not contain numeric x/y/angle"; + } + + if (!current_pose_available) { + detail = "target pose could not be verified"; + if (!query_error.empty()) { + detail += ": " + query_error; + } + return false; + } + + const double distance_error = std::hypot( + current_pose.x - context.target.x, + current_pose.y - context.target.y); + const double angle_error = angleDistance( + current_pose.theta, + context.target.theta); + std::ostringstream description; + description << "pose_source=" << pose_source + << ", current_pose=(" << current_pose.x + << "," << current_pose.y + << "," << current_pose.theta + << "), target_pose=(" << context.target.x + << "," << context.target.y + << "," << context.target.theta + << "), distance_error=" << distance_error + << ", distance_tolerance=" << context.reach_distance + << ", angle_error=" << angle_error + << ", angle_tolerance=" << context.reach_angle; + if (!query_error.empty()) { + description << ", 1004_query_error=" << query_error; + } + detail = description.str(); + return distance_error <= context.reach_distance + && angle_error <= context.reach_angle; +} + +AgvResult Src1100Agv::confirmPoseNavigationStarted_( + const PoseTaskContext& context) const { const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout; std::string last_status = "no task status received"; + int consecutive_running_samples = 0; + bool matching_task_observed = false; + int last_matching_state = 0; + bool last_poll_matched = false; + bool running_stability_window_active = false; + std::chrono::steady_clock::time_point running_stable_at{}; + const auto running_stability_window = std::chrono::milliseconds( + controllerFaultCaptureGraceMs_()); + const auto hard_deadline = deadline + running_stability_window; - const auto cached_fault_detail = [this]() { - std::lock_guard lock(runtime_state_mutex_); - if (!cached_runtime_state_valid_ || !cached_runtime_state_.fault) { - return std::string{}; - } - return cached_runtime_state_.last_error; + const auto superseded = [this, &context]() { + return navigation_generation_.load(std::memory_order_relaxed) + != context.navigation_generation; }; + const auto superseded_result = []() { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SRC1100 free-navigation start confirmation was superseded by " + "another accepted navigation, velocity, pause, or stop command; " + "the controller task state is unknown, so do not retry automatically " + "before querying or canceling navigation"); + }; + const auto fault_monitoring_unavailable = + [this, &context]() { + if (controller_fault_channel_epoch_.load( + std::memory_order_relaxed) + != context.controller_fault_channel_epoch_at_start) { + return std::string( + "the controller fault push channel changed or was " + "invalidated after the free-navigation command was " + "accepted"); + } + return freeNavigationFaultStateUnavailableDetail_(); + }; + const auto fault_monitoring_unavailable_result = + [](const std::string& detail) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 accepted the free-navigation command, but controller " + "fault monitoring became unavailable during start " + "confirmation: " + detail + + "; the task state is unsafe to accept, so query the " + "controller and cancel or stop before another motion " + "command"); + }; + const auto fault_attribution_is_ambiguous = + [&context](const std::uint64_t associated_control_attempt) { + return associated_control_attempt != 0 + && associated_control_attempt + != context.control_attempt_sequence_at_start; + }; + const auto ambiguous_fault_result = + [](const std::string& task_detail, const std::string& fault) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 reported a controller fault after another control " + "command attempt had begun; the fault " + "cannot be attributed to the tracked free-navigation task: " + + task_detail + ", " + fault + + "; query navigation status and cancel or stop before " + "another motion command"); + }; while (true) { - if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 free-navigation start confirmation was superseded by " - "another accepted navigation, velocity, pause, or stop command; " - "the controller task state is unknown, so do not retry automatically " - "before querying or canceling navigation"); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); } - Json::Value payload(Json::objectValue); - Json::Value task_ids(Json::arrayValue); - task_ids.append(task_id); - jsonMember(payload, "task_ids") = std::move(task_ids); - - Json::Value response; - const auto query_result = sendCommand_( - sock_status_, - kRobotStatusTaskPackage, - payload, - &response); + PoseTaskStatus task_status; + const auto query_result = queryPoseTaskStatus_( + context.task_id, + task_status); if (!query_result.ok()) { const std::string detail = query_result.message.empty() ? "unknown error" @@ -1775,94 +2618,275 @@ AgvResult Src1100Agv::confirmPoseNavigationStarted_( + "; do not retry automatically before checking or canceling navigation"); } - const auto controller_result = resultFromResponse_(response); - if (!controller_result.ok()) { - return AgvResult::failure( - controller_result.code, - "SRC1100 accepted the free-navigation command, but task status " - "query failed: " + controller_result.message); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); } - if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 free-navigation start confirmation was superseded by " - "another accepted navigation, velocity, pause, or stop command; " - "the controller task state is unknown, so do not retry automatically " - "before querying or canceling navigation"); - } + last_status = task_status.detail; + last_poll_matched = task_status.found; + if (task_status.found) { + matching_task_observed = true; + last_matching_state = task_status.state; + if (task_status.type != 1) { + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SRC1100 created an unexpected task type for free navigation: " + + last_status); + } - const auto* package = jsonFind(response, "task_status_package"); - const auto* status_list = package ? jsonFind(*package, "task_status_list") : nullptr; - bool matching_task_found = false; - if (status_list && status_list->isArray()) { - for (const auto& item : *status_list) { - if (jsonGet(item, "task_id", "").asString() != task_id) { - continue; + if (task_status.state == 2) { + const auto now = std::chrono::steady_clock::now(); + if (!running_stability_window_active) { + running_stability_window_active = true; + running_stable_at = now + running_stability_window; } - matching_task_found = true; - const int task_state = jsonGet(item, "status", 0).asInt(); - const int task_type = jsonGet(item, "type", 0).asInt(); - last_status = "task_id=" + task_id - + ", task_status=" + std::to_string(task_state) - + ", task_type=" + std::to_string(task_type); - if (package) { - const std::string info = jsonGet(*package, "info", "").asString(); - if (!info.empty()) { - last_status += ", info=" + info; + ++consecutive_running_samples; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (!fault.empty()) { + if (superseded()) { + return superseded_result(); + } + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); } - } - - if (task_type != 1) { return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 created an unexpected task type for free navigation: " - + last_status); + AgvErrorCode::Fault, + "SRC1100 free-navigation task reached Running state, " + "but the controller reported a new fault during start " + "confirmation: " + last_status + ", " + fault + + "; do not retry automatically; cancel or stop the " + "task before another motion command"); } - if (task_state == 1 || task_state == 2 || task_state == 4) { + if (consecutive_running_samples + >= kPoseNavigationRequiredRunningSamples + && now >= running_stable_at) { + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result( + unavailable); + } return AgvResult::success(); } - if (task_state == 3) { - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 free-navigation task was established but is paused: " - + last_status - + "; do not retry automatically before querying or canceling it"); + } else { + consecutive_running_samples = 0; + running_stability_window_active = false; + } + + if (task_status.state == 3) { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return superseded_result(); } - if (task_state == 5 || task_state == 6) { - std::string detail = last_status; - const std::string fault = cached_fault_detail(); - if (!fault.empty()) { - detail += ", " + fault; + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); } return AgvResult::failure( - task_state == 5 ? AgvErrorCode::TaskFailed : AgvErrorCode::TaskCanceled, - task_state == 5 - ? "SRC1100 free-navigation task failed: " + detail - : "SRC1100 free-navigation task was canceled: " + detail); + AgvErrorCode::Fault, + "SRC1100 free-navigation task was established but " + "paused while the controller reported a new fault: " + + last_status + ", " + fault + + "; do not retry automatically before querying or " + "canceling it"); } - break; + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SRC1100 free-navigation task was established but is paused: " + + last_status + + "; do not retry automatically before querying or canceling it"); } - } - if (!matching_task_found) { - last_status = "task_id=" + task_id + " not present in task_status_package"; + if (task_status.state == 4) { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 free-navigation task reported Completed, but " + "the controller reported a new fault during completion " + "confirmation: " + last_status + ", " + fault + + "; do not retry automatically; cancel or stop " + "the task before another motion command"); + } + std::string pose_detail; + const bool target_reached = + poseTargetReached_(context, pose_detail); + if (superseded()) { + return superseded_result(); + } + std::uint64_t post_pose_fault_control_attempt = 0; + const std::string post_pose_fault = + cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &post_pose_fault_control_attempt); + if (!post_pose_fault.empty()) { + if (fault_attribution_is_ambiguous( + post_pose_fault_control_attempt)) { + return ambiguous_fault_result( + last_status, + post_pose_fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 free-navigation task reported Completed, but " + "the controller reported a new fault during target " + "verification: " + last_status + ", " + + post_pose_fault + + "; do not retry automatically; cancel or stop " + "the task before another motion command"); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (target_reached) { + return AgvResult::success(); + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SRC1100 free-navigation task completed before a stable running " + "state, but the requested target was not reached: " + + last_status + ", " + pose_detail + + "; check the freeGo payload and controller alarms before retrying"); + } + if (task_status.state == 5 || task_status.state == 6) { + std::string detail = last_status; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return superseded_result(); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + detail += + ", controller_fault_attribution=ambiguous because " + "the fault was observed after another control " + "command attempt had begun"; + } + detail += ", " + fault; + } + return AgvResult::failure( + task_status.state == 5 + ? AgvErrorCode::TaskFailed + : AgvErrorCode::TaskCanceled, + task_status.state == 5 + ? "SRC1100 free-navigation task failed: " + detail + : "SRC1100 free-navigation task was canceled: " + detail); + } + } else { + consecutive_running_samples = 0; + running_stability_window_active = false; } - if (std::chrono::steady_clock::now() >= deadline) { + const auto now = std::chrono::steady_clock::now(); + if (now >= hard_deadline + || (now >= deadline && !running_stability_window_active)) { break; } std::this_thread::sleep_for(kPoseNavigationPollInterval); } + if (matching_task_observed + && last_poll_matched + && last_matching_state == 1) { + // A matching Waiting task has been accepted by the controller and may + // legitimately remain queued. Returning a rejection here would invite + // a duplicate command while the original task can still start later. + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 free-navigation task was accepted and remains active, " + "but the controller reported a new fault: " + + last_status + ", " + fault + + "; the task may still start later, so do not retry " + "automatically; cancel or stop it before another motion command"); + } + return AgvResult::success(); + } + std::string detail = last_status; - const std::string fault = cached_fault_detail(); + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (superseded()) { + return superseded_result(); + } if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + detail += + ", controller_fault_attribution=ambiguous because the fault " + "was observed after another control command attempt had begun"; + } detail += ", " + fault; } return AgvResult::failure( AgvErrorCode::TaskRejected, - "SRC1100 accepted the free-navigation command, but no matching pose " - "task was established within " + "SRC1100 accepted the free-navigation command, but no stable matching " + "pose task was established within " + std::to_string(kPoseNavigationStartTimeout.count()) + " ms; last " + detail + "; do not retry automatically before checking or canceling navigation"); @@ -1873,12 +2897,28 @@ AgvResult Src1100Agv::sendControlledCommand_( const std::uint16_t command, const Json::Value& payload, Json::Value* response, - std::uint64_t* accepted_navigation_generation) const + std::uint64_t* accepted_navigation_generation, + std::uint64_t* controller_fault_sequence_at_attempt, + std::uint64_t* control_attempt_sequence, + PoseTaskContext* pose_context_to_publish, + const bool reject_if_active_controller_fault) const { // Keep the permission acquisition and the following write ordered with // respect to other control RPCs in this process. Channel I/O serialization // is separate, so this must remain a distinct lock. std::lock_guard sequence_lock(control_sequence_mutex_); + const auto attempt_sequence = + control_attempt_sequence_.fetch_add( + 1, + std::memory_order_relaxed) + 1; + if (control_attempt_sequence) { + *control_attempt_sequence = attempt_sequence; + } + if (pose_context_to_publish) { + pose_context_to_publish->control_attempt_sequence_at_start = + attempt_sequence; + } + const auto authority = acquireControl_(); if (!authority.ok()) { const std::string detail = authority.message.empty() ? "unknown error" : authority.message; @@ -1886,14 +2926,79 @@ AgvResult Src1100Agv::sendControlledCommand_( authority.code, "SRC1100 acquire control authority failed: " + detail); } + std::string controller_fault_gate_error; + if (controller_fault_sequence_at_attempt + || pose_context_to_publish + || reject_if_active_controller_fault) { + std::lock_guard lock(runtime_state_mutex_); + if (controller_fault_sequence_at_attempt) { + *controller_fault_sequence_at_attempt = + controller_fault_sequence_; + } + if (pose_context_to_publish) { + pose_context_to_publish->controller_fault_sequence_at_start = + controller_fault_sequence_; + pose_context_to_publish + ->controller_fault_channel_epoch_at_start = + controller_fault_channel_epoch_.load( + std::memory_order_relaxed); + } + if (reject_if_active_controller_fault) { + if (!state_push_enabled_) { + controller_fault_gate_error = + "controller fault state is unavailable because state push " + "is disabled"; + } else if (!active_controller_fault_detail_.empty()) { + controller_fault_gate_error = + "the controller reported a fault or invalid fault state: " + + active_controller_fault_detail_; + } else if (!controller_fault_state_observed_) { + controller_fault_gate_error = + "no state push containing fatals/errors has been observed"; + } else { + const auto fault_state_age = + std::chrono::duration_cast( + std::chrono::steady_clock::now() + - controller_fault_state_observed_at_) + .count(); + if (fault_state_age > controllerFaultStateMaxAgeMs_()) { + controller_fault_gate_error = + "the most recent fatals/errors state push is stale " + "(age_ms=" + std::to_string(fault_state_age) + + ", max_age_ms=" + + std::to_string(controllerFaultStateMaxAgeMs_()) + + ")"; + } + } + } + } + if (!controller_fault_gate_error.empty()) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SRC1100 free-navigation command was not sent because " + + controller_fault_gate_error); + } + const auto publish_navigation_generation = + [this, + accepted_navigation_generation, + pose_context_to_publish]() { + const auto generation = + navigation_generation_.fetch_add( + 1, + std::memory_order_relaxed) + 1; + *accepted_navigation_generation = generation; + if (pose_context_to_publish) { + pose_context_to_publish->navigation_generation = generation; + rememberPoseTask_(*pose_context_to_publish); + } + }; auto result = sendCommand_(sock, command, payload, response); if (!result.ok()) { if (accepted_navigation_generation) { // Once the control write has been attempted, a timeout, disconnect, // wrong response opcode, or malformed JSON cannot prove rejection: // the controller may already have executed the command. - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(std::move(result)); } return result; @@ -1902,15 +3007,13 @@ AgvResult Src1100Agv::sendControlledCommand_( return result; } if (!response) { - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(AgvResult::failure( AgvErrorCode::CommandFailed, "SRC1100 cannot confirm navigation command without a response")); } if (!hasNumericControllerRetCode(*response)) { - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(resultFromResponse_(*response)); } result = resultFromResponse_(*response); @@ -1922,8 +3025,7 @@ AgvResult Src1100Agv::sendControlledCommand_( // releasing control_sequence_mutex_. This prevents a failed cancel/pause or // failed authority acquisition from falsely reporting a pose task canceled, // while preserving the controller's actual command order under concurrency. - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return result; } @@ -2158,6 +3260,7 @@ void Src1100Agv::stopPushThread_() if (push_thread_.joinable()) { push_thread_.join(); } + invalidateControllerFaultState_(); } void Src1100Agv::pushLoop_() @@ -2181,8 +3284,10 @@ void Src1100Agv::pushLoop_() } if (!result.ok()) { if (result.code != AgvErrorCode::Timeout) { + invalidateControllerFaultState_(); std::lock_guard lock(mutex_); last_error_ = result.message; + closeSocket_(sock_push_); } continue; } @@ -2193,6 +3298,7 @@ void Src1100Agv::pushLoop_() Json::Value parsed; std::string error; if (!parseJson_(payload, parsed, error)) { + invalidateControllerFaultState_(); std::lock_guard lock(mutex_); last_error_ = error; continue; @@ -2201,13 +3307,24 @@ void Src1100Agv::pushLoop_() } } +void Src1100Agv::invalidateControllerFaultState_() +{ + std::lock_guard lock(runtime_state_mutex_); + controller_fault_channel_epoch_.fetch_add( + 1, + std::memory_order_relaxed); + controller_fault_state_observed_ = false; + controller_fault_state_observed_at_ = {}; + active_controller_fault_detail_.clear(); + runtime_state_cv_.notify_all(); +} + void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) { std::lock_guard lock(runtime_state_mutex_); auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{}; state.timestamp = nowSeconds(); state.connected = true; - state.last_error.clear(); if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble(); if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble(); @@ -2244,17 +3361,65 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) } state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; - state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors"); - if (state.fault) { - std::ostringstream detail; - detail << "SRC1100 controller fault"; - if (const auto* fatals = jsonFind(payload, "fatals"); fatals && !fatals->empty()) { - detail << ": fatals=" << jsonValueToString(*fatals); + const bool has_fatals = jsonHas(payload, "fatals"); + const bool has_errors = jsonHas(payload, "errors"); + const bool has_fault_fields = has_fatals || has_errors; + if (has_fault_fields) { + const auto* fatals = jsonFind(payload, "fatals"); + const auto* errors = jsonFind(payload, "errors"); + const bool valid_fatals = !has_fatals + || (fatals && fatals->isArray()); + const bool valid_errors = !has_errors + || (errors && errors->isArray()); + const bool complete_fault_state = + has_fatals && has_errors && valid_fatals && valid_errors; + if (complete_fault_state) { + controller_fault_state_observed_ = true; + controller_fault_state_observed_at_ = + std::chrono::steady_clock::now(); + } else { + controller_fault_state_observed_ = false; + controller_fault_state_observed_at_ = {}; } - if (const auto* errors = jsonFind(payload, "errors"); errors && !errors->empty()) { - detail << ": errors=" << jsonValueToString(*errors); + + const bool reported_fault = + hasFaultArray(payload, "fatals") + || hasFaultArray(payload, "errors"); + const bool invalid_or_incomplete_fault_state = + !complete_fault_state && !reported_fault; + state.fault = reported_fault + || invalid_or_incomplete_fault_state; + if (state.fault) { + std::ostringstream detail; + detail << (reported_fault + ? "SRC1100 controller fault" + : "SRC1100 controller fault state is incomplete or malformed"); + if (fatals + && (!fatals->isArray() + || !fatals->empty() + || !complete_fault_state)) { + detail << ": fatals=" << jsonValueToString(*fatals); + } + if (errors + && (!errors->isArray() + || !errors->empty() + || !complete_fault_state)) { + detail << ": errors=" << jsonValueToString(*errors); + } + state.last_error = detail.str(); + if (state.last_error != active_controller_fault_detail_) { + active_controller_fault_detail_ = state.last_error; + ++controller_fault_sequence_; + last_controller_fault_timestamp_ = state.timestamp; + last_controller_fault_detail_ = state.last_error; + last_controller_fault_control_attempt_ = + control_attempt_sequence_.load( + std::memory_order_acquire); + } + } else { + state.last_error.clear(); + active_controller_fault_detail_.clear(); } - state.last_error = detail.str(); } if (state.emergency_stopped) { state.mode = AgvMode::EmergencyStop; @@ -2270,6 +3435,7 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) cached_runtime_state_ = state; cached_runtime_state_valid_ = true; + runtime_state_cv_.notify_all(); } std::vector Src1100Agv::buildFrame_( diff --git a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp index 5d7b4712..da0034ea 100644 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -48,6 +49,39 @@ public: agv.updateCachedRuntimeState_(payload); } + static void setAdapterError(Src1100Agv& agv, std::string error) + { + std::lock_guard lock(agv.mutex_); + agv.last_error_ = std::move(error); + } + + static void setFaultStateUnknown(Src1100Agv& agv) + { + agv.invalidateControllerFaultState_(); + } + + static void setFaultStateAge( + Src1100Agv& agv, + const std::chrono::milliseconds age) + { + std::lock_guard lock(agv.runtime_state_mutex_); + agv.controller_fault_state_observed_ = true; + agv.controller_fault_state_observed_at_ = + std::chrono::steady_clock::now() - age; + agv.active_controller_fault_detail_.clear(); + } + + static bool hasTrackedPoseTask(const Src1100Agv& agv) + { + Src1100Agv::PoseTaskContext context; + return agv.currentPoseTask_(context); + } + + static AgvResult disconnect(Src1100Agv& agv) + { + return agv.disconnect_(); + } + static void setNavigationReceiveTimeout( Src1100Agv& agv, const std::chrono::milliseconds timeout) @@ -70,6 +104,7 @@ public: namespace { constexpr std::uint16_t kRobotStatusTask = 1020; +constexpr std::uint16_t kRobotStatusLoc = 1004; constexpr std::uint16_t kRobotStatusTaskPackage = 1110; constexpr std::uint16_t kRobotControlStop = 2000; constexpr std::uint16_t kRobotControlMotion = 2010; @@ -416,6 +451,8 @@ protected: cfg.set_ip("invalid-ip"); cfg.set_recv_timeout_ms(100); cfg.set_control_nick_name("cmvr-test"); + cfg.set_enable_state_push(true); + cfg.set_state_push_interval_ms(200); controller_.setResponsePayload( kRobotStatusTask, R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})"); @@ -435,6 +472,16 @@ protected: navigation_socket, config_socket, other_socket); + Json::Value fault_state_push(Json::objectValue); + *fault_state_push.demand( + "errors", + "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *fault_state_push.demand( + "fatals", + "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_state_push); } void TearDown() override @@ -487,9 +534,17 @@ protected: TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { - expectControlledSequence({kRobotTaskGoTarget, kRobotStatusTaskPackage}, [this]() { - return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); + controller_.clearRecords(); + const auto pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(pose_result.ok()) << pose_result.message; + const auto pose_records = controller_.records(); + ASSERT_GE(pose_records.size(), 4U); + EXPECT_EQ(pose_records[0].command, kRobotConfigLock); + EXPECT_EQ(pose_records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < pose_records.size(); ++index) { + EXPECT_EQ(pose_records[index].command, kRobotStatusTaskPackage); + } expectControlled(kRobotTaskGoTarget, [this]() { return agv_->navigateToStation("station-1"); }); @@ -604,7 +659,9 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimits) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseUsesLegacyCompatibleFreeGoPayloadWithTypedMotionLimits) { AgvMotionOptions options; options.max_speed = 0.6; @@ -621,14 +678,16 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); + ASSERT_GE(records.size(), 4U); EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } const auto payload = parsePayload(records[1]); EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), ""); EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); const auto& free_go = payloadValue(payload, "freeGo"); EXPECT_TRUE(payloadValue(free_go, "x").isNumeric()); @@ -643,7 +702,9 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9); EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1); EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2); - EXPECT_FALSE(payloadHas(payload, "skill_name")); + EXPECT_EQ( + payloadValue(payload, "skill_name").asString(), + "GotoSpecifiedPose"); EXPECT_FALSE(payloadHas(payload, "x")); EXPECT_FALSE(payloadHas(payload, "y")); EXPECT_FALSE(payloadHas(payload, "angle")); @@ -657,9 +718,19 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit EXPECT_EQ( requested_task_ids[0].asString(), payloadValue(payload, "task_id").asString()); + const auto second_status_payload = parsePayload(records.back()); + const auto& second_requested_task_ids = + payloadValue(second_status_payload, "task_ids"); + ASSERT_TRUE(second_requested_task_ids.isArray()); + ASSERT_EQ(second_requested_task_ids.size(), 1U); + EXPECT_EQ( + second_requested_task_ids[0].asString(), + payloadValue(payload, "task_id").asString()); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeepsOriginFreeGo) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseUsesExplicitTaskIdAsUniquePrefixAndWhitelistsAdapterFields) { controller_.setResponsePayload( kRobotStatusTaskPackage, @@ -682,15 +753,21 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeep ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); + ASSERT_GE(records.size(), 4U); EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } const auto payload = parsePayload(records[1]); + const std::string first_task_id = + payloadValue(payload, "task_id").asString(); EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "task_id").asString().find("pose-task_pose_"), 0U); + EXPECT_EQ(payloadValue(payload, "id").asString(), ""); + EXPECT_EQ( + first_task_id.find("pose-task_pose_"), + 0U); EXPECT_EQ(payloadValue(payload, "skill_name").asString(), "GotoSpecifiedPose"); const auto& free_go = payloadValue(payload, "freeGo"); EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 0.0); @@ -700,6 +777,20 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeep EXPECT_FALSE(payloadHas(payload, "jack_height")); EXPECT_FALSE(payloadHas(payload, "script_name")); EXPECT_FALSE(payloadHas(payload, "unknown_field")); + + controller_.clearRecords(); + const auto second_result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.5}, + {}, + adapter_params); + ASSERT_TRUE(second_result.ok()) << second_result.message; + const auto second_records = controller_.records(); + ASSERT_GE(second_records.size(), 2U); + const auto second_payload = parsePayload(second_records[1]); + const std::string second_task_id = + payloadValue(second_payload, "task_id").asString(); + EXPECT_EQ(second_task_id.find("pose-task_pose_"), 0U); + EXPECT_NE(first_task_id, second_task_id); } TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill) @@ -729,7 +820,7 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAn EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("target_id must be SELF_POSITION"), std::string::npos); + EXPECT_NE(result.message.find("target_id must be empty"), std::string::npos); EXPECT_TRUE(controller_.records().empty()); adapter_params.values.clear(); @@ -747,6 +838,71 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAn EXPECT_TRUE(controller_.records().empty()); } +TEST_F( + Src1100ControlAuthorityTest, + MotionCommandsRejectNonFiniteNumericInputsBeforeAcquiringAuthority) +{ + const double nan = std::numeric_limits::quiet_NaN(); + const double infinity = std::numeric_limits::infinity(); + + controller_.clearRecords(); + auto result = agv_->navigateToPose(math::Pose2d{nan, 0.0, 0.0}); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("pose"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions pose_options; + pose_options.reach_distance = infinity; + controller_.clearRecords(); + result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.0}, + pose_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("reach_distance"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions station_options; + station_options.max_acceleration = nan; + controller_.clearRecords(); + result = agv_->navigateToStation("station-1", station_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("max_acceleration"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions negative_options; + negative_options.max_speed = -0.1; + controller_.clearRecords(); + result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.0}, + negative_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("non-negative"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvAdapterParams invalid_adapter; + invalid_adapter.values.emplace("jack_height", "inf"); + controller_.clearRecords(); + result = agv_->navigateToStation( + "station-1", + {}, + invalid_adapter); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("jack_height"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + controller_.clearRecords(); + result = agv_->setVelocity(AgvVelocity{0.0, infinity, 0.0}); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("velocity"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears) { controller_.setResponsePayload( @@ -761,13 +917,644 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntil ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); + ASSERT_GE(records.size(), 5U); EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } +} + +TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWaitsForStableRunningAfterWaiting) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_GE(records.size(), 5U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotReturnSuccessBeforeLateRunningFaultPush) +{ + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_RUNNING_31: safety controller rejected free navigation"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("E_RUNNING_31"), std::string::npos); + EXPECT_NE(result.message.find("Running state"), std::string::npos); + EXPECT_NE( + result.message.find("do not retry automatically"), + std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseFailsIfFaultPushChannelInvalidatesDuringStartConfirmation) +{ + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Src1100AgvTestPeer::setFaultStateUnknown(*agv_); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("fault monitoring became unavailable"), + std::string::npos); + EXPECT_NE( + result.message.find("push channel changed or was invalidated"), + std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsCompletedTargetWhenLateControllerFaultArrives) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_COMPLETED_45: controller rejected completed pose"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("reported Completed"), std::string::npos); + EXPECT_NE(result.message.find("E_COMPLETED_45"), std::string::npos); + const auto records = controller_.records(); + EXPECT_FALSE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + })); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsFaultArrivingDuringCompletedPoseVerification) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 700; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_POSE_VERIFY_46: fault during target verification"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("target verification"), std::string::npos); + EXPECT_NE(result.message.find("E_POSE_VERIFY_46"), std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotMisattributeFaultFromCommandAwaitingAck) +{ + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + AgvResult station_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponseDelay( + kRobotTaskGoTarget, + std::chrono::milliseconds(300)); + std::thread station_thread([this, &station_result]() { + station_result = agv_->navigateToStation("station-1"); + }); + + bool station_command_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + const auto go_target_count = std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }); + station_command_observed = go_target_count >= 2; + if (station_command_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(station_command_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_STATION_ACK_18: fault from concurrent station command"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + pose_thread.join(); + station_thread.join(); + + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); + EXPECT_NE( + pose_result.message.find("another control command attempt"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("cannot be attributed"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("E_STATION_ACK_18"), + std::string::npos); + ASSERT_TRUE(station_result.ok()) << station_result.message; +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotMisattributeFaultObservedAfterAnotherCommandAck) +{ + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponseCode(kRobotTaskGoTarget, 4188); + const auto station_result = agv_->navigateToStation("station-1"); + EXPECT_FALSE(station_result.ok()); + EXPECT_NE(station_result.message.find("ret_code=4188"), std::string::npos); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_AFTER_ACK_19: delayed station command alarm"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); + EXPECT_NE( + pose_result.message.find("another control command attempt"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("cannot be attributed"), + std::string::npos); + EXPECT_NE(pose_result.message.find("E_AFTER_ACK_19"), std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + PauseDuringPoseCommandAckPreservesPublishedTaskContext) +{ + controller_.setResponseDelay( + kRobotTaskGoTarget, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + AgvResult pause_result = AgvResult::failure( + AgvErrorCode::CommandFailed, + "pause not called"); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool pose_command_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + pose_command_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }); + if (pose_command_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(pose_command_observed); + + std::thread pause_thread([this, &pause_result]() { + pause_result = agv_->pauseNavigation(); + }); + pause_thread.join(); + pose_thread.join(); + + ASSERT_TRUE(pause_result.ok()) << pause_result.message; + EXPECT_FALSE(pose_result.ok()); + EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); + EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseReturnsAcceptedWhenMatchingTaskRemainsQueued) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + const auto status_query_count = std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + EXPECT_GT(status_query_count, 2); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseReturnsControllerFaultWhenQueuedTaskRaisesAlarm) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_WAIT_19: safety interlock"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("remains active"), std::string::npos); + EXPECT_NE(result.message.find("E_WAIT_19"), std::string::npos); + EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotReportAcceptedAfterMatchingTaskDisappears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("not present in task_status_package"), + std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotHangWhenRunningTaskDisappears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + + const auto started_at = std::chrono::steady_clock::now(); + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + const auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - started_at); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("not present in task_status_package"), + std::string::npos); + EXPECT_LT(elapsed, std::chrono::seconds(3)); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseReturnsFailureAfterWaitingTransitionsToFailed) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"create_on":"2026-07-31T10:00:00Z","err_msg":"controller task failed","task_status_package":{"closest_target":"goal-7","source_name":"SELF_POSITION","target_name":"free-goal","percentage":0.0,"distance":1.4,"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); + EXPECT_NE(result.message.find("task_status=5"), std::string::npos); + EXPECT_NE(result.message.find("status_query_ret_code=0"), std::string::npos); + EXPECT_NE(result.message.find("controller task failed"), std::string::npos); + EXPECT_NE(result.message.find("closest_target=goal-7"), std::string::npos); + EXPECT_NE(result.message.find("distance=1.400000"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 4U); EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); EXPECT_EQ(records[3].command, kRobotStatusTaskPackage); } +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsCompletedTaskWhenRequestedTargetWasNotReached) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE(result.message.find("target was not reached"), std::string::npos); + EXPECT_NE(result.message.find("distance_error=1"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 4U); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[3].command, kRobotStatusLoc); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseAcceptsCompletedTaskOnlyWhenRequestedTargetWasReached) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 4U); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[3].command, kRobotStatusLoc); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotUseFaultOnlyPushAsAValidCompletedPose) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("controller fault without pose fields"); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"err_msg":""})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("target pose could not be verified"), + std::string::npos); + EXPECT_NE( + result.message.find("did not contain numeric x/y/angle"), + std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsCompletedTaskWithNonNumericControllerPose) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":null,"y":false,"angle":"0.0"})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("did not contain numeric x/y/angle"), + std::string::npos); +} + TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure) { controller_.setResponsePayload( @@ -866,22 +1653,97 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly) EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultCodes) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPosePreservesControllerFaultWhenTaskImmediatelyPauses) { - Json::Value push_payload(Json::objectValue); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); Json::Value errors(Json::arrayValue); - Json::Value error(Json::objectValue); - *error.demand("code", "code" + std::strlen("code")) = "E_NAV_42"; - *error.demand("message", "message" + std::strlen("message")) = "planner alarm"; - errors.append(error); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); + errors.append("E_PAUSED_55: safety controller paused failed task"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("paused"), std::string::npos); + EXPECT_NE(result.message.find("E_PAUSED_55"), std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseIncludesTaskCorrelatedRawControllerFaultDetail) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_NAV_42: planner alarm"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); controller_.setResponsePayload( kRobotStatusTaskPackage, R"({"ret_code":0,"task_status_package":{"info":"navigation failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + pose_thread.join(); EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); @@ -889,6 +1751,165 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultC EXPECT_NE(result.message.find("planner alarm"), std::string::npos); } +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsPreexistingControllerFaultWithoutSendingTask) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("OLD_FAULT_FROM_PREVIOUS_TASK"); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); + controller_.clearRecords(); + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("OLD_FAULT_FROM_PREVIOUS_TASK"), + std::string::npos); + EXPECT_NE(result.message.find("was not sent"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsUnknownOrStaleFaultStateWithoutSendingTask) +{ + Src1100AgvTestPeer::setFaultStateUnknown(*agv_); + controller_.clearRecords(); + + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("no state push containing fatals/errors"), + std::string::npos); + auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + + Src1100AgvTestPeer::setFaultStateAge( + *agv_, + std::chrono::seconds(3)); + controller_.clearRecords(); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("state push is stale"), std::string::npos); + records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseRejectsMalformedFaultStateWithoutSendingTask) +{ + Json::Value malformed_push(Json::objectValue); + *malformed_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(); + *malformed_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, malformed_push); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("incomplete or malformed"), + std::string::npos); + EXPECT_NE(result.message.find("errors=null"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseDoesNotAttributeClearedFaultHistoryToNewTask) +{ + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("OLD_CLEARED_FAULT"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"new task failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("new task failed"), std::string::npos); + EXPECT_EQ(result.message.find("OLD_CLEARED_FAULT"), std::string::npos); +} + +TEST_F( + Src1100ControlAuthorityTest, + CancelSupersedesCompletedPoseWhileLocationVerificationIsInFlight) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + const auto cancel_result = agv_->cancelNavigation(); + pose_thread.join(); + + ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); +} + TEST_F(Src1100ControlAuthorityTest, CancelSupersedesPoseStartConfirmation) { controller_.setResponsePayload( @@ -955,7 +1976,7 @@ TEST_F(Src1100ControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirm } std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - ASSERT_TRUE(status_query_observed); + EXPECT_TRUE(status_query_observed); controller_.setResponseCode(kRobotConfigLock, 17); const auto authority_failure = agv_->cancelNavigation(); @@ -1290,6 +2311,419 @@ TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessag EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); } +TEST_F( + Src1100ControlAuthorityTest, + MapModeCommandsSupersedeTrackedFreeNavigation) +{ + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->switchMap("map-1"); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->startMapping(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->stopMapping(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + Src1100ControlAuthorityTest, + PauseResumeAndStopVelocityPreserveTrackedFreeNavigation) +{ + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->pauseNavigation(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->resumeNavigation(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->stopVelocityControl(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + controller_.clearRecords(); + const auto status = agv_->navigationStatus(); + EXPECT_EQ(status.state, AgvTaskState::Running); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); +} + +TEST_F(Src1100ControlAuthorityTest, NavigationStatusQueriesTrackedPoseTaskPackage) +{ + AgvAdapterParams adapter_params; + adapter_params.values.emplace("task_id", "pose-task-current"); + const auto navigate_result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + {}, + adapter_params); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + const auto navigate_records = controller_.records(); + ASSERT_GE(navigate_records.size(), 2U); + const auto navigate_payload = parsePayload(navigate_records[1]); + const std::string generated_task_id = + payloadValue(navigate_payload, "task_id").asString(); + ASSERT_FALSE(generated_task_id.empty()); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"create_on":"2026-07-31T10:00:01Z","err_msg":"","task_status_package":{"percentage":42.5,"distance":0.7,"info":"operator pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Paused); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_DOUBLE_EQ(status.progress, 42.5); + EXPECT_NE(status.message.find("task_id=" + generated_task_id), std::string::npos); + EXPECT_NE(status.message.find("operator pause"), std::string::npos); + EXPECT_NE(status.message.find("create_on=2026-07-31T10:00:01Z"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); + const auto payload = parsePayload(records[0]); + const auto& task_ids = payloadValue(payload, "task_ids"); + ASSERT_TRUE(task_ids.isArray()); + ASSERT_EQ(task_ids.size(), 1U); + EXPECT_EQ(task_ids[0].asString(), generated_task_id); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusReturnsControllerFaultWhileTaskStillReportsRunning) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_RUNNING_STATUS_52: collision input active"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("controller_task_state=2"), std::string::npos); + EXPECT_NE(status.message.find("E_RUNNING_STATUS_52"), std::string::npos); + EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusPreservesFaultWhenTrackedTaskDisappears) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"task vanished","task_status_list":[]}})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_TASK_GONE_54: controller removed failed task"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("task disappeared"), std::string::npos); + EXPECT_NE(status.message.find("E_TASK_GONE_54"), std::string::npos); + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + EXPECT_FALSE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTask; + })); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusRejectsFaultArrivingDuringCompletedPoseVerification) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 700; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_STATUS_VERIFY_53: fault during completed pose check"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("target verification"), std::string::npos); + EXPECT_NE(status.message.find("E_STATUS_VERIFY_53"), std::string::npos); + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusRejectsCompletedPoseWhenTargetWasNotReached) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE( + status.message.find("requested target was not reached"), + std::string::npos); + EXPECT_NE(status.message.find("distance_error"), std::string::npos); + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[1].command, kRobotStatusLoc); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusWaitsBrieflyForLateControllerFaultDetail) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"planner failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value fatals(Json::arrayValue); + fatals.append("E_LATE_77: localization alarm"); + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = fatals; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("planner failed"), std::string::npos); + EXPECT_NE(status.message.find("E_LATE_77"), std::string::npos); + EXPECT_NE(status.message.find("localization alarm"), std::string::npos); + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + Src1100ControlAuthorityTest, + NavigationStatusDoesNotReturnOldPoseTaskAfterStationSupersedesIt) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"old pose paused","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(300)); + controller_.setResponsePayload( + kRobotStatusTask, + R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":2,"move_status_info":"station task running"})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + const auto station_result = agv_->navigateToStation("station-1"); + status_thread.join(); + + ASSERT_TRUE(station_result.ok()) << station_result.message; + EXPECT_EQ(status.state, AgvTaskState::Running); + EXPECT_EQ(status.type, AgvTaskType::NavigateToStation); + EXPECT_EQ(status.message, "station task running"); + const auto records = controller_.records(); + EXPECT_TRUE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTask; + })); +} + +TEST_F(Src1100ControlAuthorityTest, DisconnectClearsTrackedPoseTask) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); + + const auto disconnect_result = Src1100AgvTestPeer::disconnect(*agv_); + + ASSERT_TRUE(disconnect_result.ok()) << disconnect_result.message; + EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F(Src1100ControlAuthorityTest, RuntimeStatePreservesCachedControllerFaultDetail) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + Json::Value error(Json::objectValue); + *error.demand("code", "code" + std::strlen("code")) = "E_NAV_42"; + *error.demand("message", "message" + std::strlen("message")) = "planner alarm"; + errors.append(error); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); + Src1100AgvTestPeer::setAdapterError( + *agv_, + "SRC1100 map file is empty after stripping the transport header"); + controller_.clearRecords(); + + const auto state = agv_->runtimeState(); + + EXPECT_TRUE(state.connected); + EXPECT_TRUE(state.fault); + EXPECT_EQ(state.mode, AgvMode::Fault); + EXPECT_NE(state.last_error.find("E_NAV_42"), std::string::npos); + EXPECT_NE(state.last_error.find("planner alarm"), std::string::npos); + EXPECT_NE(state.last_error.find("adapter_error="), std::string::npos); + EXPECT_NE(state.last_error.find("map file is empty"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode) { controller_.setResponseCode(kRobotStatusTask, 51020); @@ -1300,6 +2734,9 @@ TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode EXPECT_EQ(status.state, AgvTaskState::Failed); EXPECT_NE(status.message.find("ret_code=51020"), std::string::npos); EXPECT_NE(status.message.find("err_msg=simulated command failure"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTask); } } // namespace