From b5c6d50022093c0ca7e156921e1dd8f3213b259f Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 14:27:20 +0800 Subject: [PATCH] fix: restore SRC1100 free navigation protocol --- .../devices/agv/src1100/include/src1100_agv.h | 21 +- .../devices/agv/src1100/src/src1100_agv.cpp | 554 +++++++++++-- .../tests/src1100_control_authority_test.cpp | 764 +++++++++++++++++- .../grpc/tests/grpc_agv_service_test.cpp | 30 +- 4 files changed, 1265 insertions(+), 104 deletions(-) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 3d745590..3aa08913 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/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}; - int sock_control_{-1}; - int sock_navigation_{-1}; - int sock_config_{-1}; - int sock_other_{-1}; - int sock_push_{-1}; + mutable std::atomic navigation_generation_{0}; + mutable std::atomic pose_task_sequence_{0}; + mutable int sock_status_{-1}; + mutable int sock_control_{-1}; + mutable int sock_navigation_{-1}; + mutable int sock_config_{-1}; + mutable int sock_other_{-1}; + mutable int sock_push_{-1}; std::string last_error_; std::atomic push_running_{false}; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 1158473a..6c9f5ff8 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/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::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& 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 status_io_lock(status_io_mutex_); std::lock_guard lock(mutex_); closeSocket_(sock_status_); closeSocket_(sock_control_); @@ -550,6 +594,7 @@ AgvResult Src1100Agv::disconnect_() { stopMapUpdateThread_(); stopPushThread_(); + std::lock_guard status_io_lock(status_io_mutex_); std::lock_guard 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()) { - const std::string detail = motion_stop.message.empty() ? "unknown error" : motion_stop.message; - if (!navigation_cancel.ok()) { - const std::string cancel_detail = navigation_cancel.message.empty() + advance_generation_if_needed(navigation_cancel); + + if (!motion_stop.result.ok()) { + const std::string detail = motion_stop.result.message.empty() + ? "unknown error" + : motion_stop.result.message; + if (!navigation_cancel.result.ok()) { + const std::string cancel_detail = navigation_cancel.result.message.empty() ? "unknown error" - : navigation_cancel.message; + : 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()) { - const std::string detail = navigation_cancel.message.empty() + if (!navigation_cancel.result.ok()) { + 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, "y") = pose.y; - jsonMember(payload, "angle") = pose.theta; - applyMotionOptions_(payload, options, false); + 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; + } + + 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); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t navigation_generation = 0; + 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); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTarget, + payload, + &response, + &accepted_generation); } AgvResult Src1100Agv::followPath(const std::vector& path) @@ -672,41 +807,49 @@ AgvResult Src1100Agv::followPath(const std::vector& path) } jsonMember(payload, "move_task_list") = tasks; Json::Value response; - auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + 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); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &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); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &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); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &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); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( + sock_control_, + kRobotControlMotion, + payload, + &response, + &accepted_generation); } AgvResult Src1100Agv::listMaps(std::vector& 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 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 - // socket mutex, so this must remain a distinct lock. + // 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 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,28 +1961,109 @@ AgvResult Src1100Agv::sendCommandRaw_( const Json::Value& payload, std::string* response_payload) const { - std::lock_guard lock(mutex_); - if (sock < 0) { - return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected"); + const auto exchange = [&]() { + 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(frame.size())) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SRC1100 send command failed: " + systemError()); + } + + std::uint16_t response_command = 0; + std::string payload_text_response; + const auto result = receiveFrame_(sock, response_command, payload_text_response); + if (!result.ok()) { + return result; + } + const auto expected_response_command = static_cast( + 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 lock(mutex_); + if (sock < 0) { + return AgvResult::failure( + AgvErrorCode::NotConnected, + "SRC1100 socket not connected"); + } + is_status_socket = sock == sock_status_; } - 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(frame.size())) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError()); - } - - std::uint16_t response_command = 0; - std::string payload_text_response; - const auto result = receiveFrame_(sock, response_command, payload_text_response); - if (!result.ok()) { + 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 status_lock(status_io_mutex_); + { + std::lock_guard 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 lock(mutex_); + close_matching_socket_locked(); + return mark_channel_desynchronized(std::move(result)); + } return result; } - (void)response_command; - if (response_payload) { - *response_payload = std::move(payload_text_response); + + std::lock_guard 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"); } - return AgvResult::success(); + 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; } 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 f83b37ba..5d7b4712 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 @@ -1,8 +1,11 @@ #include #include +#include #include +#include #include #include +#include #include #include #include @@ -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(timeout.count() / 1000); + value.tv_usec = static_cast( + (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& data) } std::vector responseFrame( - const std::uint16_t request_command, - const int ret_code) + const std::uint16_t response_command, + 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 frame(16 + payload.size(), 0); frame[0] = 0x5A; frame[1] = 0x01; @@ -158,13 +184,45 @@ std::vector responseFrame( frame[5] = static_cast((length >> 16U) & 0xFFU); frame[6] = static_cast((length >> 8U) & 0xFFU); frame[7] = static_cast(length & 0xFFU); - const auto response_command = static_cast(request_command + 10000U); frame[8] = static_cast((response_command >> 8U) & 0xFFU); frame[9] = static_cast(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 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 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 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 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 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 lock(response_codes_mutex_); + response_commands_[request_command] = response_command; } void clearRecords() @@ -261,17 +350,48 @@ private: } { std::lock_guard 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( + command + 10000U); { std::lock_guard lock(response_codes_mutex_); - const auto response = response_codes_.find(command); - if (response != response_codes_.end()) { - ret_code = response->second; + const auto payloads = response_payloads_.find(command); + if (payloads != response_payloads_.end() && !payloads->second.empty()) { + response_payload = payloads->second.front(); + if (payloads->second.size() > 1U) { + payloads->second.pop_front(); + } + } + 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 (!sendAll(fd, responseFrame(command, ret_code))) { + 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 records_; std::mutex response_codes_mutex_; std::unordered_map response_codes_; + std::unordered_map> response_payloads_; + std::unordered_map response_delays_; + std::unordered_map 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(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), - controller_.takeClient(Channel::Navigation), - controller_.takeClient(Channel::Config), - controller_.takeClient(Channel::Other)); + status_socket, + control_socket, + navigation_socket, + 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_TRUE(payloadValue(payload, "y").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "angle").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").asDouble(), 0.5); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION"); + EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); + const auto& free_go = payloadValue(payload, "freeGo"); + EXPECT_TRUE(payloadValue(free_go, "x").isNumeric()); + 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_FALSE(payloadHas(payload, "source_id")); + 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_FALSE(payloadHas(payload, "freeGo")); - EXPECT_FALSE(payloadHas(payload, "reach_dist")); - EXPECT_FALSE(payloadHas(payload, "reach_angle")); + EXPECT_FALSE(payloadHas(payload, "x")); + EXPECT_FALSE(payloadHas(payload, "y")); + 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::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 diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp index 6a3077a8..da988fdd 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/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