fix: restore SRC1100 free navigation protocol

cmvr-es/devices/agv/src1100/include/src1100_agv.h

@@ -85,10 +85,14 @@ private:
     bool connected_() const;
 
     AgvResult acquireControl_() const;
+    AgvResult confirmPoseNavigationStarted_(
+        const std::string& task_id,
+        std::uint64_t navigation_generation) const;
     AgvResult sendControlledCommand_(int sock,
                                      std::uint16_t command,
                                      const Json::Value& payload,
-                                     Json::Value* response) const;
+                                     Json::Value* response,
+                                     std::uint64_t* accepted_navigation_generation = nullptr) const;
     AgvResult sendCommand_(int sock,
                            std::uint16_t command,
                            const Json::Value& payload,
@@ -160,13 +164,16 @@ private:
     std::size_t map_update_history_size_{8};
 
     mutable std::mutex mutex_;
+    mutable std::mutex status_io_mutex_;
     mutable std::mutex control_sequence_mutex_;
-    int sock_status_{-1};
+    mutable std::atomic<std::uint64_t> navigation_generation_{0};
-    int sock_control_{-1};
+    mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
-    int sock_navigation_{-1};
+    mutable int sock_status_{-1};
-    int sock_config_{-1};
+    mutable int sock_control_{-1};
-    int sock_other_{-1};
+    mutable int sock_navigation_{-1};
-    int sock_push_{-1};
+    mutable int sock_config_{-1};
+    mutable int sock_other_{-1};
+    mutable int sock_push_{-1};
     std::string last_error_;
 
     std::atomic<bool> push_running_{false};

cmvr-es/devices/agv/src1100/src/src1100_agv.cpp

@@ -30,6 +30,7 @@ namespace {
 constexpr std::uint16_t kRobotStatusLoc = 1004;
 constexpr std::uint16_t kRobotStatusBattery = 1007;
 constexpr std::uint16_t kRobotStatusTask = 1020;
+constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
 constexpr std::uint16_t kRobotStatusMap = 1300;
 constexpr std::uint16_t kRobotStatusStation = 1301;
 constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
@@ -40,7 +41,6 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022;
 constexpr std::uint16_t kRobotTaskPause = 3001;
 constexpr std::uint16_t kRobotTaskResume = 3002;
 constexpr std::uint16_t kRobotTaskCancel = 3003;
-constexpr std::uint16_t kRobotTaskGoPoint = 3050;
 constexpr std::uint16_t kRobotTaskGoTarget = 3051;
 constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
 constexpr std::uint16_t kRobotConfigLock = 4005;
@@ -52,6 +52,8 @@ constexpr std::uint16_t kRobotPushConfigReq = 9300;
 constexpr std::uint16_t kRobotPushConfigRes = 19300;
 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 kDefaultMapUpdateIntervalMs = 1000;
 constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
 constexpr std::uint64_t kMapSnapshotSequenceStart = 1;
@@ -171,6 +173,16 @@ bool jsonHas(const Json::Value& value, const char* key)
     return jsonFind(value, key) != nullptr;
 }
 
+bool hasNumericControllerRetCode(const Json::Value& response)
+{
+    const auto* ret_code = jsonFind(response, "ret_code");
+    return ret_code
+        && (ret_code->isInt()
+            || ret_code->isUInt()
+            || ret_code->isInt64()
+            || ret_code->isUInt64());
+}
+
 bool hasFaultArray(const Json::Value& value, const char* key)
 {
     const auto* found = jsonFind(value, key);
@@ -191,6 +203,28 @@ std::string jsonValueToString(const Json::Value& value)
     return Json::writeString(builder, value);
 }
 
+AgvResult withUnknownControllerOutcome(AgvResult result)
+{
+    const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
+    std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
+    detail +=
+        "; SRC1100 controller outcome is unknown after the command attempt; "
+        "the command may already have taken effect; do not issue another motion "
+        "command automatically; query status and cancel or stop first";
+    return AgvResult::failure(code, detail);
+}
+
+std::string makePoseTaskId(
+    const std::string& device_id,
+    const std::uint64_t task_sequence)
+{
+    const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
+    return prefix + "_pose_" + std::to_string(timestamp)
+        + "_" + std::to_string(task_sequence);
+}
+
 void putPropertyIfPresent(
     std::unordered_map<std::string, std::string>& properties,
     const Json::Value& value,
@@ -461,6 +495,12 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const
         status.message = result.message;
         return status;
     }
+    const auto controller_result = resultFromResponse_(response);
+    if (!controller_result.ok()) {
+        status.state = AgvTaskState::Failed;
+        status.message = controller_result.message;
+        return status;
+    }
 
     status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
     status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
@@ -477,6 +517,10 @@ AgvResult Src1100Agv::connect_()
     stopMapUpdateThread_();
 
     {
+        // Status requests may wait for a controller receive timeout without
+        // holding mutex_. Serialize lifecycle changes with that channel before
+        // replacing or closing its descriptor.
+        std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
         std::lock_guard<std::mutex> lock(mutex_);
         closeSocket_(sock_status_);
         closeSocket_(sock_control_);
@@ -550,6 +594,7 @@ AgvResult Src1100Agv::disconnect_()
 {
     stopMapUpdateThread_();
     stopPushThread_();
+    std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
     std::lock_guard<std::mutex> lock(mutex_);
     closeSocket_(sock_status_);
     closeSocket_(sock_control_);
@@ -575,6 +620,10 @@ AgvResult Src1100Agv::emergencyStop()
             "SRC1100 acquire control authority failed: " + detail);
     }
 
+    struct StopOutcome {
+        AgvResult result;
+        bool controller_outcome_unknown{false};
+    };
     const auto send_stop = [this](const int sock, const std::uint16_t command) {
         Json::Value response;
         auto result = sendCommand_(
@@ -582,32 +631,60 @@ AgvResult Src1100Agv::emergencyStop()
             command,
             Json::Value(Json::objectValue),
             &response);
-        return result.ok() ? resultFromResponse_(response) : result;
+        if (!result.ok()) {
+            return StopOutcome{
+                withUnknownControllerOutcome(std::move(result)),
+                true};
+        }
+        if (!hasNumericControllerRetCode(response)) {
+            return StopOutcome{
+                withUnknownControllerOutcome(resultFromResponse_(response)),
+                true};
+        }
+        return StopOutcome{resultFromResponse_(response), false};
+    };
+
+    bool generation_advanced = false;
+    const auto advance_generation_if_needed = [this, &generation_advanced](
+                                                  const StopOutcome& outcome) {
+        if (!generation_advanced
+            && (outcome.result.ok() || outcome.controller_outcome_unknown)) {
+            // Publish immediately after the first accepted or indeterminate stop
+            // outcome. Waiting for the second stop response would leave a window
+            // in which pose-start confirmation could incorrectly return success.
+            navigation_generation_.fetch_add(1, std::memory_order_relaxed);
+            generation_advanced = true;
+        }
     };
 
     const auto motion_stop = send_stop(sock_control_, kRobotControlStop);
+    advance_generation_if_needed(motion_stop);
     const auto navigation_cancel = send_stop(sock_navigation_, kRobotTaskCancel);
-    if (!motion_stop.ok()) {
+    advance_generation_if_needed(navigation_cancel);
-        const std::string detail = motion_stop.message.empty() ? "unknown error" : motion_stop.message;
+
-        if (!navigation_cancel.ok()) {
+    if (!motion_stop.result.ok()) {
-            const std::string cancel_detail = navigation_cancel.message.empty()
+        const std::string detail = motion_stop.result.message.empty()
             ? "unknown error"
-                : navigation_cancel.message;
+            : motion_stop.result.message;
+        if (!navigation_cancel.result.ok()) {
+            const std::string cancel_detail = navigation_cancel.result.message.empty()
+                ? "unknown error"
+                : navigation_cancel.result.message;
             return AgvResult::failure(
-                motion_stop.code,
+                motion_stop.result.code,
                 "SRC1100 software stop failed: control stop: " + detail
                     + "; cancel navigation: " + cancel_detail);
         }
         return AgvResult::failure(
-            motion_stop.code,
+            motion_stop.result.code,
             "SRC1100 software stop failed: control stop: " + detail);
     }
-    if (!navigation_cancel.ok()) {
+    if (!navigation_cancel.result.ok()) {
-        const std::string detail = navigation_cancel.message.empty()
+        const std::string detail = navigation_cancel.result.message.empty()
             ? "unknown error"
-            : navigation_cancel.message;
+            : navigation_cancel.result.message;
         return AgvResult::failure(
-            navigation_cancel.code,
+            navigation_cancel.result.code,
             "SRC1100 software stop failed: cancel navigation: " + detail);
     }
     return AgvResult::success();
@@ -625,20 +702,73 @@ AgvResult Src1100Agv::navigateToPose(
     const AgvMotionOptions& options,
     const AgvAdapterParams& adapter_params)
 {
-    (void)adapter_params;
+    // 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.
+    std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION");
+    if (source_id.empty()) {
+        source_id = "SELF_POSITION";
+    }
+    if (source_id != "SELF_POSITION") {
+        return AgvResult::failure(
+            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") {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SRC1100 free navigation target_id must be SELF_POSITION so a "
+            "malformed freeGo request cannot fall back to station navigation");
+    }
+
+    const auto skill_name = adapter_params.getString("skill_name");
+    if (skill_name && !skill_name->empty() && *skill_name != "GotoSpecifiedPose") {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SRC1100 free navigation skill_name must be GotoSpecifiedPose");
+    }
+
+    const auto task_sequence =
+        pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1;
+    const std::string task_id_prefix =
+        adapter_params.getString("task_id").value_or(id_);
+    const std::string task_id = makePoseTaskId(task_id_prefix, task_sequence);
 
-    // API 3050 is the controller's arbitrary world-coordinate navigation
-    // command. Do not encode a map pose as API 3051/freeGo: that extension is
-    // only defined for differential-drive chassis, and a multi-steer chassis
-    // may accept the command before the navigation task fails.
     Json::Value payload(Json::objectValue);
-    jsonMember(payload, "x") = pose.x;
+    jsonMember(payload, "source_id") = source_id;
-    jsonMember(payload, "y") = pose.y;
+    jsonMember(payload, "id") = target_id;
-    jsonMember(payload, "angle") = pose.theta;
+    jsonMember(payload, "task_id") = task_id;
-    applyMotionOptions_(payload, options, false);
+    if (skill_name && !skill_name->empty()) {
+        jsonMember(payload, "skill_name") = *skill_name;
+    }
+
+    auto& free_go = jsonMember(payload, "freeGo");
+    jsonMember(free_go, "x") = pose.x;
+    jsonMember(free_go, "y") = pose.y;
+    jsonMember(free_go, "theta") = pose.theta;
+
+    // Only strongly typed motion fields and the string whitelist above are
+    // accepted here. Generic adapter passthrough could inject unrelated 3051
+    // operations such as lift, fork, script, or digital-I/O actions.
+    applyMotionOptions_(payload, options);
+
     Json::Value response;
-    auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoPoint, payload, &response);
+    std::uint64_t navigation_generation = 0;
-    return result.ok() ? resultFromResponse_(response) : result;
+    auto result = sendControlledCommand_(
+        sock_navigation_,
+        kRobotTaskGoTarget,
+        payload,
+        &response,
+        &navigation_generation);
+    if (!result.ok()) {
+        return result;
+    }
+    return confirmPoseNavigationStarted_(task_id, navigation_generation);
 }
 
 AgvResult Src1100Agv::navigateToStation(
@@ -654,8 +784,13 @@ AgvResult Src1100Agv::navigateToStation(
     // extensions so the SRC controller receives JSON numbers.
     applyMotionOptions_(payload, options);
     Json::Value response;
-    auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response);
+    std::uint64_t accepted_generation = 0;
-    return result.ok() ? resultFromResponse_(response) : result;
+    return sendControlledCommand_(
+        sock_navigation_,
+        kRobotTaskGoTarget,
+        payload,
+        &response,
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::followPath(const std::vector<AgvPathSegment>& path)
@@ -672,41 +807,49 @@ AgvResult Src1100Agv::followPath(const std::vector<AgvPathSegment>& path)
     }
     jsonMember(payload, "move_task_list") = tasks;
     Json::Value response;
-    auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response);
+    std::uint64_t accepted_generation = 0;
-    return result.ok() ? resultFromResponse_(response) : result;
+    return sendControlledCommand_(
+        sock_navigation_,
+        kRobotTaskGoTargetList,
+        payload,
+        &response,
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::pauseNavigation()
 {
     Json::Value response;
-    auto result = sendControlledCommand_(
+    std::uint64_t accepted_generation = 0;
+    return sendControlledCommand_(
         sock_navigation_,
         kRobotTaskPause,
         Json::Value(Json::objectValue),
-        &response);
+        &response,
-    return result.ok() ? resultFromResponse_(response) : result;
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::resumeNavigation()
 {
     Json::Value response;
-    auto result = sendControlledCommand_(
+    std::uint64_t accepted_generation = 0;
+    return sendControlledCommand_(
         sock_navigation_,
         kRobotTaskResume,
         Json::Value(Json::objectValue),
-        &response);
+        &response,
-    return result.ok() ? resultFromResponse_(response) : result;
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::cancelNavigation()
 {
     Json::Value response;
-    auto result = sendControlledCommand_(
+    std::uint64_t accepted_generation = 0;
+    return sendControlledCommand_(
         sock_navigation_,
         kRobotTaskCancel,
         Json::Value(Json::objectValue),
-        &response);
+        &response,
-    return result.ok() ? resultFromResponse_(response) : result;
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity)
@@ -716,8 +859,13 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity)
     jsonMember(payload, "vy") = velocity.vy;
     jsonMember(payload, "w") = velocity.wz;
     Json::Value response;
-    auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response);
+    std::uint64_t accepted_generation = 0;
-    return result.ok() ? resultFromResponse_(response) : result;
+    return sendControlledCommand_(
+        sock_control_,
+        kRobotControlMotion,
+        payload,
+        &response,
+        &accepted_generation);
 }
 
 AgvResult Src1100Agv::listMaps(std::vector<std::string>& maps) const
@@ -1580,15 +1728,156 @@ AgvResult Src1100Agv::acquireControl_() const
     return result.ok() ? resultFromResponse_(response) : result;
 }
 
+AgvResult Src1100Agv::confirmPoseNavigationStarted_(
+    const std::string& task_id,
+    const std::uint64_t navigation_generation) const
+{
+    const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout;
+    std::string last_status = "no task status received";
+
+    const auto cached_fault_detail = [this]() {
+        std::lock_guard<std::mutex> lock(runtime_state_mutex_);
+        if (!cached_runtime_state_valid_ || !cached_runtime_state_.fault) {
+            return std::string{};
+        }
+        return cached_runtime_state_.last_error;
+    };
+
+    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");
+        }
+
+        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);
+        if (!query_result.ok()) {
+            const std::string detail = query_result.message.empty()
+                ? "unknown error"
+                : query_result.message;
+            return AgvResult::failure(
+                query_result.code,
+                "SRC1100 accepted the free-navigation command, but task start "
+                "could not be verified: " + detail
+                    + "; 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 (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");
+        }
+
+        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;
+                }
+                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;
+                    }
+                }
+
+                if (task_type != 1) {
+                    return AgvResult::failure(
+                        AgvErrorCode::TaskRejected,
+                        "SRC1100 created an unexpected task type for free navigation: "
+                            + last_status);
+                }
+                if (task_state == 1 || task_state == 2 || task_state == 4) {
+                    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");
+                }
+                if (task_state == 5 || task_state == 6) {
+                    std::string detail = last_status;
+                    const std::string fault = cached_fault_detail();
+                    if (!fault.empty()) {
+                        detail += ", " + 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);
+                }
+                break;
+            }
+        }
+        if (!matching_task_found) {
+            last_status = "task_id=" + task_id + " not present in task_status_package";
+        }
+
+        if (std::chrono::steady_clock::now() >= deadline) {
+            break;
+        }
+        std::this_thread::sleep_for(kPoseNavigationPollInterval);
+    }
+
+    std::string detail = last_status;
+    const std::string fault = cached_fault_detail();
+    if (!fault.empty()) {
+        detail += ", " + fault;
+    }
+    return AgvResult::failure(
+        AgvErrorCode::TaskRejected,
+        "SRC1100 accepted the free-navigation command, but no matching pose "
+        "task was established within "
+            + std::to_string(kPoseNavigationStartTimeout.count())
+            + " ms; last " + detail
+            + "; do not retry automatically before checking or canceling navigation");
+}
+
 AgvResult Src1100Agv::sendControlledCommand_(
     const int sock,
     const std::uint16_t command,
     const Json::Value& payload,
-    Json::Value* response) const
+    Json::Value* response,
+    std::uint64_t* accepted_navigation_generation) const
 {
     // Keep the permission acquisition and the following write ordered with
-    // respect to other control RPCs in this process. sendCommand_ has its own
+    // respect to other control RPCs in this process. Channel I/O serialization
-    // socket mutex, so this must remain a distinct lock.
+    // is separate, so this must remain a distinct lock.
     std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
     const auto authority = acquireControl_();
     if (!authority.ok()) {
@@ -1597,7 +1886,45 @@ AgvResult Src1100Agv::sendControlledCommand_(
             authority.code,
             "SRC1100 acquire control authority failed: " + detail);
     }
-    return sendCommand_(sock, command, payload, response);
+    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;
+            return withUnknownControllerOutcome(std::move(result));
+        }
+        return result;
+    }
+    if (!accepted_navigation_generation) {
+        return result;
+    }
+    if (!response) {
+        *accepted_navigation_generation =
+            navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
+        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;
+        return withUnknownControllerOutcome(resultFromResponse_(*response));
+    }
+    result = resultFromResponse_(*response);
+    if (!result.ok()) {
+        return result;
+    }
+
+    // Advance only after the controller accepted the command, and do it before
+    // 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;
+    return result;
 }
 
 AgvResult Src1100Agv::sendCommand_(
@@ -1634,15 +1961,14 @@ AgvResult Src1100Agv::sendCommandRaw_(
     const Json::Value& payload,
     std::string* response_payload) const
 {
-    std::lock_guard<std::mutex> lock(mutex_);
+    const auto exchange = [&]() {
-    if (sock < 0) {
-        return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected");
-    }
-
         const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
         const auto frame = buildFrame_(command, payload_text);
-    if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
+        if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL)
-        return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError());
+            != static_cast<ssize_t>(frame.size())) {
+            return AgvResult::failure(
+                AgvErrorCode::CommandFailed,
+                "SRC1100 send command failed: " + systemError());
         }
 
         std::uint16_t response_command = 0;
@@ -1651,11 +1977,93 @@ AgvResult Src1100Agv::sendCommandRaw_(
         if (!result.ok()) {
             return result;
         }
-    (void)response_command;
+        const auto expected_response_command = static_cast<std::uint16_t>(
+            command + 10000U);
+        if (response_command != expected_response_command) {
+            return AgvResult::failure(
+                AgvErrorCode::CommandFailed,
+                "SRC1100 response command mismatch: expected="
+                    + std::to_string(expected_response_command)
+                    + ", actual=" + std::to_string(response_command));
+        }
         if (response_payload) {
             *response_payload = std::move(payload_text_response);
         }
         return AgvResult::success();
+    };
+    const auto close_matching_socket_locked = [this, sock]() {
+        if (sock == sock_status_) {
+            closeSocket_(sock_status_);
+        } else if (sock == sock_control_) {
+            closeSocket_(sock_control_);
+        } else if (sock == sock_navigation_) {
+            closeSocket_(sock_navigation_);
+        } else if (sock == sock_config_) {
+            closeSocket_(sock_config_);
+        } else if (sock == sock_other_) {
+            closeSocket_(sock_other_);
+        }
+    };
+    const auto mark_channel_desynchronized = [](AgvResult result) {
+        std::string detail = result.message.empty()
+            ? "unknown transport or frame error"
+            : result.message;
+        detail +=
+            "; SRC1100 channel closed because the response stream may be "
+            "desynchronized; reconnect before sending another command";
+        return AgvResult::failure(result.code, detail);
+    };
+
+    bool is_status_socket = false;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (sock < 0) {
+            return AgvResult::failure(
+                AgvErrorCode::NotConnected,
+                "SRC1100 socket not connected");
+        }
+        is_status_socket = sock == sock_status_;
+    }
+
+    if (is_status_socket) {
+        // A slow 1110 status response must never hold the lifecycle/global I/O
+        // mutex needed by cancelNavigation() or emergencyStop(). The dedicated
+        // status lock still serializes requests on port 19204. connect_() and
+        // disconnect_() take this lock before changing the descriptor.
+        std::lock_guard<std::mutex> status_lock(status_io_mutex_);
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (sock < 0 || sock != sock_status_) {
+                return AgvResult::failure(
+                    AgvErrorCode::NotConnected,
+                    "SRC1100 status socket is no longer connected");
+            }
+        }
+        auto result = exchange();
+        if (!result.ok()) {
+            std::lock_guard<std::mutex> lock(mutex_);
+            close_matching_socket_locked();
+            return mark_channel_desynchronized(std::move(result));
+        }
+        return result;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (sock < 0
+        || (sock != sock_control_
+            && sock != sock_navigation_
+            && sock != sock_config_
+            && sock != sock_other_)) {
+        return AgvResult::failure(
+            AgvErrorCode::NotConnected,
+            "SRC1100 socket is no longer connected");
+    }
+    auto result = exchange();
+    if (!result.ok()) {
+        close_matching_socket_locked();
+        return mark_channel_desynchronized(std::move(result));
+    }
+    return result;
 }
 
 AgvResult Src1100Agv::sendCommandNoResponse_(
@@ -1837,6 +2245,17 @@ 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);
+        }
+        if (const auto* errors = jsonFind(payload, "errors"); errors && !errors->empty()) {
+            detail << ": errors=" << jsonValueToString(*errors);
+        }
+        state.last_error = detail.str();
+    }
     if (state.emergency_stopped) {
         state.mode = AgvMode::EmergencyStop;
     } else if (state.fault) {
@@ -1995,12 +2414,21 @@ void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParam
 
 AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response)
 {
-    const int ret_code = jsonGet(response, "ret_code", 0).asInt();
+    if (!hasNumericControllerRetCode(response)) {
+        return AgvResult::failure(
+            AgvErrorCode::CommandFailed,
+            "SRC1100 controller response is missing a numeric ret_code");
+    }
+    const auto* ret_code_value = jsonFind(response, "ret_code");
+    const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
+        ? ret_code_value->asUInt64() == 0
+        : ret_code_value->asInt64() == 0;
+    const std::string ret_code = jsonValueToString(*ret_code_value);
     const std::string message = jsonGet(response, "err_msg", "").asString();
-    if (ret_code == 0) {
+    if (success) {
         return AgvResult::success();
     }
-    std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(ret_code);
+    std::string detail = "SRC1100 command failed: ret_code=" + ret_code;
     if (!message.empty()) {
         detail += ", err_msg=" + message;
     }

cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp

@@ -1,8 +1,11 @@
 #include <algorithm>
 #include <array>
+#include <atomic>
 #include <cerrno>
+#include <chrono>
 #include <cstdint>
 #include <cstring>
+#include <deque>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -27,27 +30,53 @@ class Src1100AgvTestPeer {
 public:
     static void installSockets(
         Src1100Agv& agv,
+        const int status,
         const int control,
         const int navigation,
         const int config,
         const int other)
     {
+        agv.sock_status_ = status;
         agv.sock_control_ = control;
         agv.sock_navigation_ = navigation;
         agv.sock_config_ = config;
         agv.sock_other_ = other;
     }
+
+    static void cacheRuntimeState(Src1100Agv& agv, const Json::Value& payload)
+    {
+        agv.updateCachedRuntimeState_(payload);
+    }
+
+    static void setNavigationReceiveTimeout(
+        Src1100Agv& agv,
+        const std::chrono::milliseconds timeout)
+    {
+        timeval value{};
+        value.tv_sec = static_cast<time_t>(timeout.count() / 1000);
+        value.tv_usec = static_cast<suseconds_t>(
+            (timeout.count() % 1000) * 1000);
+        ASSERT_EQ(
+            ::setsockopt(
+                agv.sock_navigation_,
+                SOL_SOCKET,
+                SO_RCVTIMEO,
+                &value,
+                sizeof(value)),
+            0);
+    }
 };
 
 namespace {
 
+constexpr std::uint16_t kRobotStatusTask = 1020;
+constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
 constexpr std::uint16_t kRobotControlStop = 2000;
 constexpr std::uint16_t kRobotControlMotion = 2010;
 constexpr std::uint16_t kRobotControlLoadMap = 2022;
 constexpr std::uint16_t kRobotTaskPause = 3001;
 constexpr std::uint16_t kRobotTaskResume = 3002;
 constexpr std::uint16_t kRobotTaskCancel = 3003;
-constexpr std::uint16_t kRobotTaskGoPoint = 3050;
 constexpr std::uint16_t kRobotTaskGoTarget = 3051;
 constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
 constexpr std::uint16_t kRobotConfigLock = 4005;
@@ -57,7 +86,8 @@ constexpr std::uint16_t kRobotOtherStartMapping = 6100;
 constexpr std::uint16_t kRobotOtherStopMapping = 6101;
 
 enum class Channel : std::size_t {
-    Control = 0,
+    Status = 0,
+    Control,
     Navigation,
     Config,
     Other,
@@ -142,13 +172,9 @@ bool sendAll(const int fd, const std::vector<std::uint8_t>& data)
 }
 
 std::vector<std::uint8_t> responseFrame(
-    const std::uint16_t request_command,
+    const std::uint16_t response_command,
-    const int ret_code)
+    const std::string& payload)
 {
-    const std::string payload = ret_code == 0
-        ? R"({"ret_code":0,"err_msg":""})"
-        : "{\"ret_code\":" + std::to_string(ret_code)
-            + R"(,"err_msg":"simulated command failure"})";
     std::vector<std::uint8_t> frame(16 + payload.size(), 0);
     frame[0] = 0x5A;
     frame[1] = 0x01;
@@ -158,13 +184,45 @@ std::vector<std::uint8_t> responseFrame(
     frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
     frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
     frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
-    const auto response_command = static_cast<std::uint16_t>(request_command + 10000U);
     frame[8] = static_cast<std::uint8_t>((response_command >> 8U) & 0xFFU);
     frame[9] = static_cast<std::uint8_t>(response_command & 0xFFU);
     std::copy(payload.begin(), payload.end(), frame.begin() + 16);
     return frame;
 }
 
+std::string injectRequestedTaskId(
+    std::string response_payload,
+    const std::string& request_payload)
+{
+    constexpr char kTaskIdToken[] = "${TASK_ID}";
+    const auto token_position = response_payload.find(kTaskIdToken);
+    if (token_position == std::string::npos) {
+        return response_payload;
+    }
+
+    Json::Value request;
+    Json::CharReaderBuilder builder;
+    std::string error;
+    std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+    if (!reader->parse(
+            request_payload.data(),
+            request_payload.data() + request_payload.size(),
+            &request,
+            &error)) {
+        return response_payload;
+    }
+    const auto* task_ids = request.find("task_ids", "task_ids" + std::strlen("task_ids"));
+    if (!task_ids || !task_ids->isArray() || task_ids->empty()) {
+        return response_payload;
+    }
+
+    response_payload.replace(
+        token_position,
+        std::strlen(kTaskIdToken),
+        (*task_ids)[0].asString());
+    return response_payload;
+}
+
 class FakeSrc1100Controller {
 public:
     FakeSrc1100Controller()
@@ -220,6 +278,37 @@ public:
     {
         std::lock_guard<std::mutex> lock(response_codes_mutex_);
         response_codes_[command] = ret_code;
+        response_payloads_.erase(command);
+    }
+
+    void setResponsePayload(const std::uint16_t command, std::string payload)
+    {
+        std::lock_guard<std::mutex> lock(response_codes_mutex_);
+        response_codes_.erase(command);
+        response_payloads_[command] = {std::move(payload)};
+    }
+
+    void queueResponsePayload(const std::uint16_t command, std::string payload)
+    {
+        std::lock_guard<std::mutex> lock(response_codes_mutex_);
+        response_codes_.erase(command);
+        response_payloads_[command].push_back(std::move(payload));
+    }
+
+    void setResponseDelay(
+        const std::uint16_t command,
+        const std::chrono::milliseconds delay)
+    {
+        std::lock_guard<std::mutex> lock(response_codes_mutex_);
+        response_delays_[command] = delay;
+    }
+
+    void setResponseCommand(
+        const std::uint16_t request_command,
+        const std::uint16_t response_command)
+    {
+        std::lock_guard<std::mutex> lock(response_codes_mutex_);
+        response_commands_[request_command] = response_command;
     }
 
     void clearRecords()
@@ -261,17 +350,48 @@ private:
             }
             {
                 std::lock_guard<std::mutex> lock(records_mutex_);
-                records_.push_back({command, std::move(payload)});
+                records_.push_back({command, payload});
             }
             int ret_code = 0;
+            std::string response_payload;
+            std::chrono::milliseconds response_delay{0};
+            std::uint16_t response_command = static_cast<std::uint16_t>(
+                command + 10000U);
             {
                 std::lock_guard<std::mutex> lock(response_codes_mutex_);
-                const auto response = response_codes_.find(command);
+                const auto payloads = response_payloads_.find(command);
-                if (response != response_codes_.end()) {
+                if (payloads != response_payloads_.end() && !payloads->second.empty()) {
-                    ret_code = response->second;
+                    response_payload = payloads->second.front();
+                    if (payloads->second.size() > 1U) {
+                        payloads->second.pop_front();
                     }
                 }
-            if (!sendAll(fd, responseFrame(command, ret_code))) {
+                const auto response_code = response_codes_.find(command);
+                if (response_code != response_codes_.end()) {
+                    ret_code = response_code->second;
+                }
+                const auto delay = response_delays_.find(command);
+                if (delay != response_delays_.end()) {
+                    response_delay = delay->second;
+                }
+                const auto response_command_override = response_commands_.find(command);
+                if (response_command_override != response_commands_.end()) {
+                    response_command = response_command_override->second;
+                }
+            }
+            if (response_delay.count() > 0) {
+                std::this_thread::sleep_for(response_delay);
+            }
+            if (response_payload.empty()) {
+                response_payload = ret_code == 0
+                    ? R"({"ret_code":0,"err_msg":""})"
+                    : "{\"ret_code\":" + std::to_string(ret_code)
+                        + R"(,"err_msg":"simulated command failure"})";
+            }
+            response_payload = injectRequestedTaskId(
+                std::move(response_payload),
+                payload);
+            if (!sendAll(fd, responseFrame(response_command, response_payload))) {
                 return;
             }
         }
@@ -282,6 +402,9 @@ private:
     std::vector<CommandRecord> records_;
     std::mutex response_codes_mutex_;
     std::unordered_map<std::uint16_t, int> response_codes_;
+    std::unordered_map<std::uint16_t, std::deque<std::string>> response_payloads_;
+    std::unordered_map<std::uint16_t, std::chrono::milliseconds> response_delays_;
+    std::unordered_map<std::uint16_t, std::uint16_t> response_commands_;
 };
 
 class Src1100ControlAuthorityTest : public ::testing::Test {
@@ -293,13 +416,25 @@ protected:
         cfg.set_ip("invalid-ip");
         cfg.set_recv_timeout_ms(100);
         cfg.set_control_nick_name("cmvr-test");
+        controller_.setResponsePayload(
+            kRobotStatusTask,
+            R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})");
+        controller_.setResponsePayload(
+            kRobotStatusTaskPackage,
+            R"({"ret_code":0,"err_msg":"","task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})");
         agv_ = std::make_unique<Src1100Agv>(cfg);
+        const int status_socket = controller_.takeClient(Channel::Status);
+        const int control_socket = controller_.takeClient(Channel::Control);
+        const int navigation_socket = controller_.takeClient(Channel::Navigation);
+        const int config_socket = controller_.takeClient(Channel::Config);
+        const int other_socket = controller_.takeClient(Channel::Other);
         Src1100AgvTestPeer::installSockets(
             *agv_,
-            controller_.takeClient(Channel::Control),
+            status_socket,
-            controller_.takeClient(Channel::Navigation),
+            control_socket,
-            controller_.takeClient(Channel::Config),
+            navigation_socket,
-            controller_.takeClient(Channel::Other));
+            config_socket,
+            other_socket);
     }
 
     void TearDown() override
@@ -352,7 +487,7 @@ protected:
 
 TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst)
 {
-    expectControlled(kRobotTaskGoPoint, [this]() {
+    expectControlledSequence({kRobotTaskGoTarget, kRobotStatusTaskPackage}, [this]() {
         return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
     });
     expectControlled(kRobotTaskGoTarget, [this]() {
@@ -469,7 +604,7 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority)
     EXPECT_EQ(records[0].command, kRobotConfigDownloadMap);
 }
 
-TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimits)
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimits)
 {
     AgvMotionOptions options;
     options.max_speed = 0.6;
@@ -486,28 +621,579 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimi
 
     ASSERT_TRUE(result.ok()) << result.message;
     const auto records = controller_.records();
-    ASSERT_EQ(records.size(), 2U);
+    ASSERT_EQ(records.size(), 3U);
     EXPECT_EQ(records[0].command, kRobotConfigLock);
-    EXPECT_EQ(records[1].command, kRobotTaskGoPoint);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+    EXPECT_EQ(records[2].command, kRobotStatusTaskPackage);
 
     const auto payload = parsePayload(records[1]);
-    EXPECT_TRUE(payloadValue(payload, "x").isNumeric());
+    EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION");
-    EXPECT_TRUE(payloadValue(payload, "y").isNumeric());
+    EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION");
-    EXPECT_TRUE(payloadValue(payload, "angle").isNumeric());
+    EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty());
-    EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0);
+    const auto& free_go = payloadValue(payload, "freeGo");
-    EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0);
+    EXPECT_TRUE(payloadValue(free_go, "x").isNumeric());
-    EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").asDouble(), 0.5);
+    EXPECT_TRUE(payloadValue(free_go, "y").isNumeric());
+    EXPECT_TRUE(payloadValue(free_go, "theta").isNumeric());
+    EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 1.0);
+    EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 2.0);
+    EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5);
     EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6);
     EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7);
     EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8);
     EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9);
-    EXPECT_FALSE(payloadHas(payload, "id"));
+    EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1);
-    EXPECT_FALSE(payloadHas(payload, "source_id"));
+    EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2);
     EXPECT_FALSE(payloadHas(payload, "skill_name"));
-    EXPECT_FALSE(payloadHas(payload, "freeGo"));
+    EXPECT_FALSE(payloadHas(payload, "x"));
-    EXPECT_FALSE(payloadHas(payload, "reach_dist"));
+    EXPECT_FALSE(payloadHas(payload, "y"));
-    EXPECT_FALSE(payloadHas(payload, "reach_angle"));
+    EXPECT_FALSE(payloadHas(payload, "angle"));
     EXPECT_FALSE(payloadHas(payload, "jack_height"));
+
+    const auto status_payload = parsePayload(records[2]);
+    const auto& requested_task_ids = payloadValue(status_payload, "task_ids");
+    ASSERT_TRUE(requested_task_ids.isArray());
+    ASSERT_EQ(requested_task_ids.size(), 1U);
+    ASSERT_TRUE(requested_task_ids[0].isString());
+    EXPECT_EQ(
+        requested_task_ids[0].asString(),
+        payloadValue(payload, "task_id").asString());
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeepsOriginFreeGo)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})");
+    AgvAdapterParams adapter_params;
+    adapter_params.values.emplace("source_id", "SELF_POSITION");
+    adapter_params.values.emplace("target_id", "SELF_POSITION");
+    adapter_params.values.emplace("task_id", "pose-task");
+    adapter_params.values.emplace("skill_name", "GotoSpecifiedPose");
+    adapter_params.values.emplace("operation", "JackHeight");
+    adapter_params.values.emplace("jack_height", "0.5");
+    adapter_params.values.emplace("script_name", "unsafe-script");
+    adapter_params.values.emplace("unknown_field", "unsafe-value");
+    controller_.clearRecords();
+
+    const auto result = agv_->navigateToPose(
+        math::Pose2d{0.0, 0.0, 0.5},
+        {},
+        adapter_params);
+
+    ASSERT_TRUE(result.ok()) << result.message;
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 3U);
+    EXPECT_EQ(records[0].command, kRobotConfigLock);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+    EXPECT_EQ(records[2].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, "task_id").asString().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);
+    EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 0.0);
+    EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5);
+    EXPECT_FALSE(payloadHas(payload, "operation"));
+    EXPECT_FALSE(payloadHas(payload, "jack_height"));
+    EXPECT_FALSE(payloadHas(payload, "script_name"));
+    EXPECT_FALSE(payloadHas(payload, "unknown_field"));
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill)
+{
+    AgvAdapterParams adapter_params;
+    adapter_params.values.emplace("source_id", "station-0");
+    controller_.clearRecords();
+
+    auto result = agv_->navigateToPose(
+        math::Pose2d{1.0, 2.0, 0.5},
+        {},
+        adapter_params);
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument);
+    EXPECT_NE(result.message.find("source_id must be SELF_POSITION"), std::string::npos);
+    EXPECT_TRUE(controller_.records().empty());
+
+    adapter_params.values.clear();
+    adapter_params.values.emplace("target_id", "station-1");
+    controller_.clearRecords();
+
+    result = agv_->navigateToPose(
+        math::Pose2d{1.0, 2.0, 0.5},
+        {},
+        adapter_params);
+
+    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_TRUE(controller_.records().empty());
+
+    adapter_params.values.clear();
+    adapter_params.values.emplace("skill_name", "unsafe-custom-skill");
+    controller_.clearRecords();
+
+    result = agv_->navigateToPose(
+        math::Pose2d{1.0, 2.0, 0.5},
+        {},
+        adapter_params);
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument);
+    EXPECT_NE(result.message.find("skill_name must be GotoSpecifiedPose"), std::string::npos);
+    EXPECT_TRUE(controller_.records().empty());
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"old-pose-task","status":2,"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_EQ(records.size(), 4U);
+    EXPECT_EQ(records[0].command, kRobotConfigLock);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+    EXPECT_EQ(records[2].command, kRobotStatusTaskPackage);
+    EXPECT_EQ(records[3].command, kRobotStatusTaskPackage);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"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("task_type=1"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesSynchronousControllerCode)
+{
+    controller_.setResponseCode(kRobotTaskGoTarget, 43051);
+    controller_.clearRecords();
+
+    const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(result.message.find("ret_code=43051"), std::string::npos);
+    EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 2U);
+    EXPECT_EQ(records[0].command, kRobotConfigLock);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesStatusQueryControllerCode)
+{
+    controller_.setResponseCode(kRobotStatusTaskPackage, 41110);
+    controller_.clearRecords();
+
+    const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(result.message.find("ret_code=41110"), std::string::npos);
+    EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsWrongResponseCommand)
+{
+    controller_.setResponseCommand(kRobotTaskGoTarget, 13052);
+    controller_.clearRecords();
+
+    const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(result.message.find("expected=13051"), std::string::npos);
+    EXPECT_NE(result.message.find("actual=13052"), std::string::npos);
+    EXPECT_NE(result.message.find("channel closed"), std::string::npos);
+    EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos);
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 2U);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsMissingControllerCode)
+{
+    controller_.setResponsePayload(
+        kRobotTaskGoTarget,
+        R"({"err_msg":"missing acknowledgment code"})");
+    controller_.clearRecords();
+
+    const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(result.message.find("missing a numeric ret_code"), std::string::npos);
+    EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos);
+    const auto records = controller_.records();
+    ASSERT_EQ(records.size(), 2U);
+    EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly)
+{
+    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();
+
+    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("established but is paused"), std::string::npos);
+    EXPECT_NE(result.message.find("safety pause"), std::string::npos);
+    EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultCodes)
+{
+    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);
+    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});
+
+    EXPECT_FALSE(result.ok());
+    EXPECT_EQ(result.code, AgvErrorCode::TaskFailed);
+    EXPECT_NE(result.message.find("E_NAV_42"), std::string::npos);
+    EXPECT_NE(result.message.find("planner alarm"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, CancelSupersedesPoseStartConfirmation)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
+    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));
+    }
+
+    const auto cancel_result = agv_->cancelNavigation();
+    pose_thread.join();
+
+    EXPECT_TRUE(status_query_observed);
+    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);
+    EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirmation)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
+    controller_.clearRecords();
+    AgvResult pose_result = AgvResult::success();
+    std::atomic_bool pose_finished{false};
+
+    std::thread pose_thread([this, &pose_result, &pose_finished]() {
+        pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+        pose_finished.store(true, std::memory_order_release);
+    });
+
+    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));
+    }
+    ASSERT_TRUE(status_query_observed);
+
+    controller_.setResponseCode(kRobotConfigLock, 17);
+    const auto authority_failure = agv_->cancelNavigation();
+    EXPECT_FALSE(authority_failure.ok());
+    std::this_thread::sleep_for(std::chrono::milliseconds(75));
+    EXPECT_FALSE(pose_finished.load(std::memory_order_acquire));
+
+    controller_.setResponseCode(kRobotConfigLock, 0);
+    controller_.setResponseCode(kRobotTaskCancel, 23);
+    const auto command_failure = agv_->cancelNavigation();
+    EXPECT_FALSE(command_failure.ok());
+    std::this_thread::sleep_for(std::chrono::milliseconds(75));
+    EXPECT_FALSE(pose_finished.load(std::memory_order_acquire));
+
+    controller_.setResponseCode(kRobotTaskCancel, 0);
+    const auto successful_cancel = agv_->cancelNavigation();
+    pose_thread.join();
+
+    ASSERT_TRUE(successful_cancel.ok()) << successful_cancel.message;
+    EXPECT_TRUE(pose_finished.load(std::memory_order_acquire));
+    EXPECT_FALSE(pose_result.ok());
+    EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(pose_result.message.find("superseded"), std::string::npos);
+    EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, IndeterminateCancelSupersedesPoseStartConfirmation)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
+    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));
+    }
+    ASSERT_TRUE(status_query_observed);
+
+    controller_.setResponsePayload(
+        kRobotTaskCancel,
+        R"({"err_msg":"acknowledgment lost"})");
+    const auto cancel_result = agv_->cancelNavigation();
+    pose_thread.join();
+
+    EXPECT_FALSE(cancel_result.ok());
+    EXPECT_NE(cancel_result.message.find("missing a numeric ret_code"), std::string::npos);
+    EXPECT_NE(cancel_result.message.find("controller outcome is unknown"), std::string::npos);
+    EXPECT_NE(
+        cancel_result.message.find("do not issue another motion command automatically"),
+        std::string::npos);
+    EXPECT_FALSE(pose_result.ok());
+    EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
+}
+
+TEST_F(Src1100ControlAuthorityTest, TimedOutChannelIsClosedBeforeSameCommandCanRetry)
+{
+    Src1100AgvTestPeer::setNavigationReceiveTimeout(
+        *agv_,
+        std::chrono::milliseconds(50));
+    controller_.setResponseDelay(
+        kRobotTaskCancel,
+        std::chrono::milliseconds(200));
+    controller_.clearRecords();
+
+    const auto first_result = agv_->cancelNavigation();
+    const auto second_result = agv_->cancelNavigation();
+
+    EXPECT_FALSE(first_result.ok());
+    EXPECT_EQ(first_result.code, AgvErrorCode::Timeout);
+    EXPECT_NE(first_result.message.find("channel closed"), std::string::npos);
+    EXPECT_NE(
+        first_result.message.find("controller outcome is unknown"),
+        std::string::npos);
+    EXPECT_FALSE(second_result.ok());
+    EXPECT_EQ(second_result.code, AgvErrorCode::NotConnected);
+    EXPECT_NE(
+        second_result.message.find("controller outcome is unknown"),
+        std::string::npos);
+
+    const auto records = controller_.records();
+    const auto cancel_count = std::count_if(
+        records.begin(),
+        records.end(),
+        [](const CommandRecord& record) {
+            return record.command == kRobotTaskCancel;
+        });
+    EXPECT_EQ(cancel_count, 1);
+}
+
+TEST_F(Src1100ControlAuthorityTest, SlowTaskStatusDoesNotBlockEmergencyStop)
+{
+    controller_.setResponsePayload(
+        kRobotStatusTaskPackage,
+        R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
+    controller_.setResponseDelay(
+        kRobotStatusTaskPackage,
+        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 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));
+    }
+    ASSERT_TRUE(status_query_observed);
+
+    const auto start = std::chrono::steady_clock::now();
+    const auto stop_result = agv_->emergencyStop();
+    const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::steady_clock::now() - start);
+    pose_thread.join();
+
+    ASSERT_TRUE(stop_result.ok()) << stop_result.message;
+    EXPECT_LT(elapsed.count(), 150);
+    EXPECT_FALSE(pose_result.ok());
+    EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
+
+    const auto records = controller_.records();
+    const auto control_stop = std::find_if(
+        records.begin(),
+        records.end(),
+        [](const CommandRecord& record) {
+            return record.command == kRobotControlStop;
+        });
+    const auto navigation_cancel = std::find_if(
+        records.begin(),
+        records.end(),
+        [](const CommandRecord& record) {
+            return record.command == kRobotTaskCancel;
+        });
+    ASSERT_NE(control_stop, records.end());
+    ASSERT_NE(navigation_cancel, records.end());
+    EXPECT_LT(control_stop, navigation_cancel);
+}
+
+TEST_F(Src1100ControlAuthorityTest, EmergencyStopInvalidatesPoseAfterFirstAcceptedStop)
+{
+    controller_.setResponseDelay(
+        kRobotStatusTaskPackage,
+        std::chrono::milliseconds(200));
+    controller_.setResponseDelay(
+        kRobotTaskCancel,
+        std::chrono::milliseconds(400));
+    controller_.clearRecords();
+    AgvResult pose_result = AgvResult::success();
+    AgvResult stop_result = AgvResult::success();
+    std::atomic_bool pose_finished{false};
+    std::atomic_bool stop_finished{false};
+
+    std::thread pose_thread([this, &pose_result, &pose_finished]() {
+        pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
+        pose_finished.store(true, std::memory_order_release);
+    });
+
+    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));
+    }
+    ASSERT_TRUE(status_query_observed);
+
+    std::thread stop_thread([this, &stop_result, &stop_finished]() {
+        stop_result = agv_->emergencyStop();
+        stop_finished.store(true, std::memory_order_release);
+    });
+
+    bool control_stop_observed = false;
+    for (int attempt = 0; attempt < 200; ++attempt) {
+        const auto records = controller_.records();
+        control_stop_observed = std::any_of(
+            records.begin(),
+            records.end(),
+            [](const CommandRecord& record) {
+                return record.command == kRobotControlStop;
+            });
+        if (control_stop_observed) {
+            break;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    ASSERT_TRUE(control_stop_observed);
+
+    for (int attempt = 0; attempt < 350; ++attempt) {
+        if (pose_finished.load(std::memory_order_acquire)) {
+            break;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    ASSERT_TRUE(pose_finished.load(std::memory_order_acquire));
+    EXPECT_FALSE(stop_finished.load(std::memory_order_acquire));
+    EXPECT_FALSE(pose_result.ok());
+    EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
+    EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
+
+    stop_thread.join();
+    pose_thread.join();
+    ASSERT_TRUE(stop_result.ok()) << stop_result.message;
 }
 
 TEST_F(Src1100ControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions)
@@ -604,5 +1290,17 @@ TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessag
     EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
 }
 
+TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode)
+{
+    controller_.setResponseCode(kRobotStatusTask, 51020);
+    controller_.clearRecords();
+
+    const auto status = agv_->navigationStatus();
+
+    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);
+}
+
 } // namespace
 } // namespace cmvr::device

cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp

@@ -14,6 +14,8 @@ namespace {
 
 constexpr char kNativeErrorMessage[] =
     "SRC1100 command failed: ret_code=41200, err_msg=speed_illegal";
+constexpr char kNativeNavigationErrorMessage[] =
+    "SRC1100 command failed: ret_code=43051, err_msg=planner_rejected_pose";
 
 class FakeAgv final : public device::AbstractAGV {
 public:
@@ -31,7 +33,7 @@ public:
     {
         pose_ = pose;
         pose_options_ = options;
-        return device::AgvResult::success();
+        return pose_result_;
     }
 
     device::AgvResult navigateToStation(
@@ -53,6 +55,7 @@ public:
 
     math::Pose2d pose_;
     device::AgvMotionOptions pose_options_;
+    device::AgvResult pose_result_{device::AgvResult::success()};
     std::string station_id_;
     device::AgvMotionOptions station_options_;
 };
@@ -159,5 +162,30 @@ TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage)
     EXPECT_EQ(response.header().error_message(), kNativeErrorMessage);
 }
 
+TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage)
+{
+    agv_->pose_result_ = device::AgvResult::failure(
+        device::AgvErrorCode::CommandFailed,
+        kNativeNavigationErrorMessage);
+    api::AgvNavigateToPoseCommand_Request request;
+    request.mutable_header()->set_device_id("test-agv");
+    request.mutable_pose()->set_x(1.0);
+    request.mutable_pose()->set_y(2.0);
+    api::AgvNavigateToPoseCommand_Feedback response;
+    grpc::ServerContext context;
+
+    const auto status = service_->navigateToPose(
+        &context,
+        &request,
+        &response);
+
+    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
+    EXPECT_EQ(status.error_message(), kNativeNavigationErrorMessage);
+    EXPECT_FALSE(response.header().success());
+    EXPECT_EQ(
+        response.header().error_message(),
+        kNativeNavigationErrorMessage);
+}
+
 } // namespace
 } // namespace cmvr::service