fix: acquire SRC1100 control authority before commands

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"

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()

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};

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<std::mutex> 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<AgvPathSegment>& 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<std::mutex> 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,

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

@@ -0,0 +1,435 @@
+#include <algorithm>
+#include <array>
+#include <cerrno>
+#include <cstdint>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <gtest/gtest.h>
+#include <json/json.h>
+
+#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<std::uint8_t*>(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<std::size_t>(count);
+            continue;
+        }
+        if (count < 0 && errno == EINTR) {
+            continue;
+        }
+        return false;
+    }
+    return true;
+}
+
+bool sendAll(const int fd, const std::vector<std::uint8_t>& 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<std::size_t>(count);
+            continue;
+        }
+        if (count < 0 && errno == EINTR) {
+            continue;
+        }
+        return false;
+    }
+    return true;
+}
+
+std::vector<std::uint8_t> 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<std::uint8_t> frame(16 + payload.size(), 0);
+    frame[0] = 0x5A;
+    frame[1] = 0x01;
+    frame[3] = 0x01;
+    const auto length = static_cast<std::uint32_t>(payload.size());
+    frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
+    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;
+}
+
+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<std::size_t>(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<std::mutex> lock(response_codes_mutex_);
+        response_codes_[command] = ret_code;
+    }
+
+    void clearRecords()
+    {
+        std::lock_guard<std::mutex> lock(records_mutex_);
+        records_.clear();
+    }
+
+    std::vector<CommandRecord> records() const
+    {
+        std::lock_guard<std::mutex> 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<std::uint8_t, 16> header{};
+            if (!receiveExact(fd, header.data(), header.size())) {
+                return;
+            }
+            const auto length = (static_cast<std::uint32_t>(header[4]) << 24U)
+                | (static_cast<std::uint32_t>(header[5]) << 16U)
+                | (static_cast<std::uint32_t>(header[6]) << 8U)
+                | static_cast<std::uint32_t>(header[7]);
+            const auto command = static_cast<std::uint16_t>(
+                (static_cast<std::uint16_t>(header[8]) << 8U) | header[9]);
+            std::string payload(length, '\0');
+            if (length > 0 && !receiveExact(fd, payload.data(), payload.size())) {
+                return;
+            }
+            {
+                std::lock_guard<std::mutex> lock(records_mutex_);
+                records_.push_back({command, std::move(payload)});
+            }
+            int ret_code = 0;
+            {
+                std::lock_guard<std::mutex> 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<Endpoint, static_cast<std::size_t>(Channel::Count)> endpoints_;
+    mutable std::mutex records_mutex_;
+    std::vector<CommandRecord> records_;
+    std::mutex response_codes_mutex_;
+    std::unordered_map<std::uint16_t, int> 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<Src1100Agv>(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<std::uint16_t>& commands,
+        const std::function<AgvResult()>& 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<Json::CharReader> 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<AgvResult()>& invoke)
+    {
+        expectControlledSequence({command}, invoke);
+    }
+
+    FakeSrc1100Controller controller_;
+    std::unique_ptr<Src1100Agv> 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

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:<device-id>"。
+  string control_nick_name = 19;
 }
 
 // 单个 AGV 设备配置。