From 31b2d98625fd90fc934b1f54244bfaf1ab940f87 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 11:12:35 +0800 Subject: [PATCH] fix: acquire SRC1100 control authority before commands --- cmvr-es/config/devices/agv/src1100.pb.txt | 1 + cmvr-es/devices/agv/src1100/CMakeLists.txt | 28 ++ .../devices/agv/src1100/include/src1100_agv.h | 11 + .../devices/agv/src1100/src/src1100_agv.cpp | 127 ++++- .../tests/src1100_control_authority_test.cpp | 435 ++++++++++++++++++ .../cmvr/config/agv_config/agv_config.proto | 2 + 6 files changed, 591 insertions(+), 13 deletions(-) create mode 100644 cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp diff --git a/cmvr-es/config/devices/agv/src1100.pb.txt b/cmvr-es/config/devices/agv/src1100.pb.txt index 833bde58..16867ce7 100644 --- a/cmvr-es/config/devices/agv/src1100.pb.txt +++ b/cmvr-es/config/devices/agv/src1100.pb.txt @@ -18,6 +18,7 @@ agv { port_other: 19210 port_push: 19301 recv_timeout_ms: 1000 + control_nick_name: "cmvr-es" enable_state_push: true state_push_interval_ms: 200 state_push_included_fields: "x" diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt index 6ad1f3fe..705542e2 100644 --- a/cmvr-es/devices/agv/src1100/CMakeLists.txt +++ b/cmvr-es/devices/agv/src1100/CMakeLists.txt @@ -10,3 +10,31 @@ target_link_libraries(src1100_agv add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv) install(TARGETS src1100_agv LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(src1100_control_authority_test + tests/src1100_control_authority_test.cpp + ) + target_link_libraries(src1100_control_authority_test + PRIVATE + cmvr_es::device::src1100_agv + gtest + gtest_main + pthread + ) + add_test( + NAME src1100_control_authority_test + COMMAND src1100_control_authority_test + ) + set(_src1100_control_authority_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _src1100_control_authority_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(src1100_control_authority_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_src1100_control_authority_test_environment}" + ) +endif() diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index ab5bcb92..466c6ced 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -17,6 +17,8 @@ namespace cmvr::device { +class Src1100AgvTestPeer; + class Src1100Agv final : public AbstractAGV { public: explicit Src1100Agv(const config::Src1100AgvConfig& cfg); @@ -64,6 +66,8 @@ public: AgvResult stopMapping() override; private: + friend class Src1100AgvTestPeer; + struct Ports { int status{19204}; int control{19205}; @@ -80,6 +84,11 @@ private: void closeSocket_(int& sock) const; bool connected_() const; + AgvResult acquireControl_() const; + AgvResult sendControlledCommand_(int sock, + std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -141,6 +150,7 @@ private: config::Src1100AgvConfig config_; std::string ip_; + std::string control_nick_name_; int recv_timeout_ms_{1000}; Ports ports_; bool state_push_enabled_{false}; @@ -149,6 +159,7 @@ private: std::size_t map_update_history_size_{8}; mutable std::mutex mutex_; + mutable std::mutex control_sequence_mutex_; int sock_status_{-1}; int sock_control_{-1}; int sock_navigation_{-1}; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 7c1b1b2b..eb26fdc2 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -42,6 +42,7 @@ constexpr std::uint16_t kRobotTaskResume = 3002; constexpr std::uint16_t kRobotTaskCancel = 3003; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotConfigLock = 4005; constexpr std::uint16_t kRobotConfigUploadMap = 4010; constexpr std::uint16_t kRobotConfigDownloadMap = 4011; constexpr std::uint16_t kRobotOtherStartMapping = 6100; @@ -339,6 +340,10 @@ AgvTaskType toTaskType(const int value) Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) : config_(cfg), ip_(cfg.ip()), + control_nick_name_( + cfg.control_nick_name().empty() + ? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id()) + : cfg.control_nick_name()), recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000), state_push_enabled_(cfg.enable_state_push()), map_update_enabled_(cfg.enable_map_update()), @@ -556,12 +561,62 @@ AgvResult Src1100Agv::disconnect_() AgvResult Src1100Agv::emergencyStop() { - return cancelNavigation(); + // This is a controller-level software stop, not a substitute for the + // physical emergency-stop circuit. Keep both stop commands under one + // authority acquisition so no other command from this process can + // interleave between them. + std::lock_guard sequence_lock(control_sequence_mutex_); + const auto authority = acquireControl_(); + if (!authority.ok()) { + const std::string detail = authority.message.empty() ? "unknown error" : authority.message; + return AgvResult::failure( + authority.code, + "SRC1100 acquire control authority failed: " + detail); + } + + const auto send_stop = [this](const int sock, const std::uint16_t command) { + Json::Value response; + auto result = sendCommand_( + sock, + command, + Json::Value(Json::objectValue), + &response); + return result.ok() ? resultFromResponse_(response) : result; + }; + + const auto motion_stop = send_stop(sock_control_, kRobotControlStop); + 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() + ? "unknown error" + : navigation_cancel.message; + return AgvResult::failure( + motion_stop.code, + "SRC1100 software stop failed: control stop: " + detail + + "; cancel navigation: " + cancel_detail); + } + return AgvResult::failure( + motion_stop.code, + "SRC1100 software stop failed: control stop: " + detail); + } + if (!navigation_cancel.ok()) { + const std::string detail = navigation_cancel.message.empty() + ? "unknown error" + : navigation_cancel.message; + return AgvResult::failure( + navigation_cancel.code, + "SRC1100 software stop failed: cancel navigation: " + detail); + } + return AgvResult::success(); } AgvResult Src1100Agv::clearFault() { - return AgvResult::success(); + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "SRC1100 clearFault command is not implemented"); } AgvResult Src1100Agv::navigateToPose( @@ -580,7 +635,7 @@ AgvResult Src1100Agv::navigateToPose( applyMotionOptions_(payload, options); applyAdapterParams_(payload, adapter_params); Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -595,7 +650,7 @@ AgvResult Src1100Agv::navigateToStation( applyMotionOptions_(payload, options); applyAdapterParams_(payload, adapter_params); Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -613,28 +668,40 @@ AgvResult Src1100Agv::followPath(const std::vector& path) } jsonMember(payload, "move_task_list") = tasks; Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::pauseNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskPause, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::resumeNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskResume, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::cancelNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskCancel, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -646,7 +713,7 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) jsonMember(payload, "w") = velocity.wz; jsonMember(payload, "duration") = -1; Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlMotion, payload, &response); + auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -690,7 +757,7 @@ AgvResult Src1100Agv::switchMap(const std::string& map_name) Json::Value payload(Json::objectValue); jsonMember(payload, "map_name") = map_name; Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlLoadMap, payload, &response); + auto result = sendControlledCommand_(sock_control_, kRobotControlLoadMap, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -700,7 +767,7 @@ AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& jsonMember(payload, "map_name") = map_name; jsonMember(payload, "map_content") = content; Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); + auto result = sendControlledCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -728,7 +795,7 @@ AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) } Json::Value response; - result = sendCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); + result = sendControlledCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); result = result.ok() ? resultFromResponse_(response) : result; if (result.ok()) { { @@ -1440,7 +1507,11 @@ AgvResult Src1100Agv::stopMapping() if (!result.ok()) return result; Json::Value response; - result = sendCommand_(sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), &response); + result = sendControlledCommand_( + sock_other_, + kRobotOtherStopMapping, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -1496,6 +1567,36 @@ bool Src1100Agv::connected_() const return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; } +AgvResult Src1100Agv::acquireControl_() const +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "nick_name") = control_nick_name_; + + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::sendControlledCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response) 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. + std::lock_guard sequence_lock(control_sequence_mutex_); + const auto authority = acquireControl_(); + if (!authority.ok()) { + const std::string detail = authority.message.empty() ? "unknown error" : authority.message; + return AgvResult::failure( + authority.code, + "SRC1100 acquire control authority failed: " + detail); + } + return sendCommand_(sock, command, payload, response); +} + AgvResult Src1100Agv::sendCommand_( const int sock, const std::uint16_t command, 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 new file mode 100644 index 00000000..cc834dd1 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -0,0 +1,435 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "devices/agv/src1100/include/src1100_agv.h" + +namespace cmvr::device { + +class Src1100AgvTestPeer { +public: + static void installSockets( + Src1100Agv& agv, + const int control, + const int navigation, + const int config, + const int other) + { + agv.sock_control_ = control; + agv.sock_navigation_ = navigation; + agv.sock_config_ = config; + agv.sock_other_ = other; + } +}; + +namespace { + +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 kRobotTaskGoTarget = 3051; +constexpr std::uint16_t kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotConfigLock = 4005; +constexpr std::uint16_t kRobotConfigUploadMap = 4010; +constexpr std::uint16_t kRobotConfigDownloadMap = 4011; +constexpr std::uint16_t kRobotOtherStartMapping = 6100; +constexpr std::uint16_t kRobotOtherStopMapping = 6101; + +enum class Channel : std::size_t { + Control = 0, + Navigation, + Config, + Other, + Count +}; + +struct CommandRecord { + std::uint16_t command{0}; + std::string payload; +}; + +bool receiveExact(const int fd, void* output, const std::size_t size) +{ + auto* bytes = static_cast(output); + std::size_t offset = 0; + while (offset < size) { + const auto count = ::recv(fd, bytes + offset, size - offset, 0); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count < 0 && errno == EINTR) { + continue; + } + return false; + } + return true; +} + +bool sendAll(const int fd, const std::vector& data) +{ + std::size_t offset = 0; + while (offset < data.size()) { + const auto count = ::send( + fd, + data.data() + offset, + data.size() - offset, + MSG_NOSIGNAL); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count < 0 && errno == EINTR) { + continue; + } + return false; + } + return true; +} + +std::vector responseFrame( + const std::uint16_t request_command, + const int ret_code) +{ + 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; + frame[3] = 0x01; + const auto length = static_cast(payload.size()); + frame[4] = static_cast((length >> 24U) & 0xFFU); + 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; +} + +class FakeSrc1100Controller { +public: + FakeSrc1100Controller() + { + for (auto& endpoint : endpoints_) { + int pair[2]{-1, -1}; + if (::socketpair(AF_UNIX, SOCK_STREAM, 0, pair) != 0) { + throw std::runtime_error("socketpair failed"); + } + endpoint.client = pair[0]; + endpoint.server = pair[1]; + } + for (std::size_t index = 0; index < endpoints_.size(); ++index) { + endpoints_[index].worker = std::thread( + &FakeSrc1100Controller::serve, + this, + index); + } + } + + ~FakeSrc1100Controller() + { + for (auto& endpoint : endpoints_) { + if (endpoint.client >= 0) { + ::shutdown(endpoint.client, SHUT_RDWR); + ::close(endpoint.client); + endpoint.client = -1; + } + if (endpoint.server >= 0) { + ::shutdown(endpoint.server, SHUT_RDWR); + } + } + for (auto& endpoint : endpoints_) { + if (endpoint.worker.joinable()) { + endpoint.worker.join(); + } + if (endpoint.server >= 0) { + ::close(endpoint.server); + endpoint.server = -1; + } + } + } + + int takeClient(const Channel channel) + { + auto& endpoint = endpoints_[static_cast(channel)]; + const int client = endpoint.client; + endpoint.client = -1; + return client; + } + + void setResponseCode(const std::uint16_t command, const int ret_code) + { + std::lock_guard lock(response_codes_mutex_); + response_codes_[command] = ret_code; + } + + void clearRecords() + { + std::lock_guard lock(records_mutex_); + records_.clear(); + } + + std::vector records() const + { + std::lock_guard lock(records_mutex_); + return records_; + } + +private: + struct Endpoint { + int client{-1}; + int server{-1}; + std::thread worker; + }; + + void serve(const std::size_t index) + { + const int fd = endpoints_[index].server; + while (true) { + std::array header{}; + if (!receiveExact(fd, header.data(), header.size())) { + return; + } + const auto length = (static_cast(header[4]) << 24U) + | (static_cast(header[5]) << 16U) + | (static_cast(header[6]) << 8U) + | static_cast(header[7]); + const auto command = static_cast( + (static_cast(header[8]) << 8U) | header[9]); + std::string payload(length, '\0'); + if (length > 0 && !receiveExact(fd, payload.data(), payload.size())) { + return; + } + { + std::lock_guard lock(records_mutex_); + records_.push_back({command, std::move(payload)}); + } + int ret_code = 0; + { + std::lock_guard lock(response_codes_mutex_); + const auto response = response_codes_.find(command); + if (response != response_codes_.end()) { + ret_code = response->second; + } + } + if (!sendAll(fd, responseFrame(command, ret_code))) { + return; + } + } + } + + std::array(Channel::Count)> endpoints_; + mutable std::mutex records_mutex_; + std::vector records_; + std::mutex response_codes_mutex_; + std::unordered_map response_codes_; +}; + +class Src1100ControlAuthorityTest : public ::testing::Test { +protected: + void SetUp() override + { + config::Src1100AgvConfig cfg; + cfg.set_id("src1100"); + cfg.set_ip("invalid-ip"); + cfg.set_recv_timeout_ms(100); + cfg.set_control_nick_name("cmvr-test"); + agv_ = std::make_unique(cfg); + Src1100AgvTestPeer::installSockets( + *agv_, + controller_.takeClient(Channel::Control), + controller_.takeClient(Channel::Navigation), + controller_.takeClient(Channel::Config), + controller_.takeClient(Channel::Other)); + } + + void TearDown() override + { + agv_.reset(); + } + + void expectControlledSequence( + const std::vector& commands, + const std::function& invoke) + { + controller_.clearRecords(); + const auto result = invoke(); + ASSERT_TRUE(result.ok()) << result.message; + + const auto records = controller_.records(); + ASSERT_EQ(records.size(), commands.size() + 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + for (std::size_t index = 0; index < commands.size(); ++index) { + EXPECT_EQ(records[index + 1U].command, commands[index]); + } + + Json::Value lock_payload; + Json::CharReaderBuilder builder; + std::string error; + std::unique_ptr reader(builder.newCharReader()); + ASSERT_TRUE(reader->parse( + records[0].payload.data(), + records[0].payload.data() + records[0].payload.size(), + &lock_payload, + &error)) << error; + constexpr char kNickName[] = "nick_name"; + const auto* nick_name = lock_payload.find( + kNickName, + kNickName + std::strlen(kNickName)); + ASSERT_NE(nick_name, nullptr); + EXPECT_EQ(nick_name->asString(), "cmvr-test"); + } + + void expectControlled( + const std::uint16_t command, + const std::function& invoke) + { + expectControlledSequence({command}, invoke); + } + + FakeSrc1100Controller controller_; + std::unique_ptr agv_; +}; + +TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) +{ + expectControlled(kRobotTaskGoTarget, [this]() { + return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + expectControlled(kRobotTaskGoTarget, [this]() { + return agv_->navigateToStation("station-1"); + }); + expectControlled(kRobotTaskGoTargetList, [this]() { + return agv_->followPath({AgvPathSegment{"station-1", "station-2"}}); + }); + expectControlled(kRobotTaskPause, [this]() { + return agv_->pauseNavigation(); + }); + expectControlled(kRobotTaskResume, [this]() { + return agv_->resumeNavigation(); + }); + expectControlled(kRobotTaskCancel, [this]() { + return agv_->cancelNavigation(); + }); + expectControlledSequence({kRobotControlStop, kRobotTaskCancel}, [this]() { + return agv_->emergencyStop(); + }); + expectControlled(kRobotControlMotion, [this]() { + return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2}); + }); + expectControlled(kRobotControlMotion, [this]() { + return agv_->stopVelocityControl(); + }); + expectControlled(kRobotControlLoadMap, [this]() { + return agv_->switchMap("map-1"); + }); + expectControlled(kRobotConfigUploadMap, [this]() { + return agv_->uploadMap("map-1", "{}"); + }); + expectControlled(kRobotOtherStartMapping, [this]() { + return agv_->startMapping(); + }); + expectControlled(kRobotOtherStopMapping, [this]() { + return agv_->stopMapping(); + }); +} + +TEST_F(Src1100ControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) +{ + controller_.setResponseCode(kRobotConfigLock, 40020); + controller_.clearRecords(); + + const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F(Src1100ControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendStopCommands) +{ + controller_.setResponseCode(kRobotConfigLock, 40020); + controller_.clearRecords(); + + const auto result = agv_->emergencyStop(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F(Src1100ControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesFailures) +{ + controller_.setResponseCode(kRobotControlStop, 50001); + controller_.setResponseCode(kRobotTaskCancel, 50002); + controller_.clearRecords(); + + const auto result = agv_->emergencyStop(); + + EXPECT_FALSE(result.ok()); + EXPECT_NE(result.message.find("control stop"), std::string::npos); + EXPECT_NE(result.message.find("cancel navigation"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 3U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotControlStop); + EXPECT_EQ(records[2].command, kRobotTaskCancel); +} + +TEST_F(Src1100ControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority) +{ + controller_.clearRecords(); + + const auto result = agv_->clearFault(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) +{ + controller_.clearRecords(); + std::string content; + + const auto result = agv_->downloadMap("map-1", content); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); +} + +} // namespace +} // namespace cmvr::device diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index c8ec8c56..10dae43b 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -49,6 +49,8 @@ message Src1100AgvConfig { int32 map_update_interval_ms = 17; // 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。 uint32 map_update_history_size = 18; + // 抢占 SRC1100 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:"。 + string control_nick_name = 19; } // 单个 AGV 设备配置。