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

#include "service/grpc/include/grpc_arm_service.h"

#include <chrono>
#include <cstdio>
#include <condition_variable>
#include <functional>
#include <future>
#include <memory>
#include <mutex>
#include <optional>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>

#include <google/protobuf/descriptor.h>
#include <grpcpp/grpcpp.h>
#include <gtest/gtest.h>
#include <unistd.h>

#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "manager/control_authority/include/control_authority_manager.h"
#include "manager/device_manager/include/device_manager.h"
#include "service/grpc/include/grpc_error_logging_interceptor.h"
#include "service/stop_all/include/stop_all_admission_gate.h"

namespace cmvr::service {
namespace {

class JsonCommandRobotArm final : public device::RobotArm {
public:
    explicit JsonCommandRobotArm(std::string id)
    {
        id_ = std::move(id);
    }

    std::string typeName() const override { return "JsonCommandRobotArm"; }

    bool executeJsonCommand(const std::string& request_json,
                            std::string& response_json) override
    {
        ++execute_calls;
        last_request_json = request_json;
        response_json = next_response_json;
        return next_success;
    }

    device::RobotModel getRobotModel() const override { return {}; }
    std::size_t getDof() const override { return 0U; }
    device::ArmState getRobotState() const override { return {}; }
    device::JointGroupState getJointState() const override { return {}; }
    device::CartesianPose getTcpPose(
        device::FrameType = device::FrameType::Base) const override
    {
        return {};
    }
    device::RobotMode getRobotMode() const override
    {
        return device::RobotMode::Unknown;
    }
    device::SafetyMode getSafetyMode() const override
    {
        return device::SafetyMode::Unknown;
    }
    device::ControlMode getControlMode() const override
    {
        return device::ControlMode::None;
    }

    device::Result torqueOn() override { return torqueOn({}); }
    device::Result torqueOn(
        const std::function<bool()>& cancellation_requested) override
    {
        std::unique_lock lock(motion_mutex_);
        ++torque_on_calls_;
        last_torque_on_had_cancellation_ =
            static_cast<bool>(cancellation_requested);
        if (!block_next_torque_on_) {
            return device::Result::success();
        }

        block_next_torque_on_ = false;
        blocking_torque_on_started_ = true;
        torque_on_started_cv_.notify_all();
        while (!release_blocking_torque_on_) {
            lock.unlock();
            const bool cancelled =
                cancellation_requested && cancellation_requested();
            lock.lock();
            if (cancelled) {
                last_torque_on_cancellation_requested_ = true;
                if (fail_next_torque_on_cancellation_) {
                    fail_next_torque_on_cancellation_ = false;
                    return device::Result::failure(
                        device::ArmErrorCode::CommandFailed,
                        "simulated torqueOn safety termination failure");
                }
                return device::Result::failure(
                    device::ArmErrorCode::CommandRejected,
                    "simulated torqueOn cancellation");
            }
            torque_on_release_cv_.wait_for(
                lock, std::chrono::milliseconds(5));
        }
        return device::Result::success();
    }
    device::Result torqueOff() override
    {
        std::lock_guard lock(motion_mutex_);
        ++torque_off_calls_;
        return device::Result::success();
    }
    device::Result calibrateZeroQ(const std::string&) override
    {
        return device::Result::success();
    }
    device::Result emergencyStop() override
    {
        return device::Result::success();
    }
    device::Result protectiveStop() override
    {
        return device::Result::success();
    }
    device::Result setSpeedScaling(double) override
    {
        return device::Result::success();
    }
    double getSpeedScaling() const override { return 1.0; }
    bool isProtectiveStopped() const override { return false; }
    bool isEmergencyStopped() const override { return false; }
    bool isFault() const override { return false; }

    device::Result moveJ(const device::JointPositionCommand&,
                         const device::MotionOptions& options) override
    {
        return enterMotion("moveJ", move_j_calls_, options);
    }
    device::Result speedJ(const device::JointVelocityCommand&,
                          double,
                          double) override
    {
        return device::Result::success();
    }
    device::Result stopJ(double) override
    {
        return device::Result::success();
    }
    device::Result moveL(
        const device::CartesianPose&,
        const device::MotionOptions& options,
        device::FrameType = device::FrameType::Base) override
    {
        return enterMotion("moveL", move_l_calls_, options);
    }
    device::Result speedL(
        const device::CartesianVelocity&,
        double,
        double,
        device::FrameType = device::FrameType::Base) override
    {
        return device::Result::success();
    }
    device::Result stopL(std::optional<double> = std::nullopt) override
    {
        return device::Result::success();
    }
    device::Result stopMotion() override
    {
        std::unique_lock lock(motion_mutex_);
        ++stop_motion_calls_;
        if (block_next_stop_) {
            block_next_stop_ = false;
            blocking_stop_started_ = true;
            stop_started_cv_.notify_all();
            stop_release_cv_.wait(
                lock,
                [this]() { return release_blocking_stop_; });
        }
        if (throw_next_stop_) {
            throw_next_stop_ = false;
            throw std::runtime_error("simulated stopMotion exception");
        }
        if (fail_next_stop_) {
            fail_next_stop_ = false;
            return device::Result::failure(
                device::ArmErrorCode::CommandFailed,
                "simulated stopMotion failure");
        }
        return device::Result::success();
    }

    void blockNextMotion()
    {
        std::lock_guard lock(motion_mutex_);
        block_next_motion_ = true;
        blocking_motion_started_ = false;
        release_blocking_motion_ = false;
        blocking_motion_name_.clear();
    }

    void blockNextTorqueOn()
    {
        std::lock_guard lock(motion_mutex_);
        block_next_torque_on_ = true;
        blocking_torque_on_started_ = false;
        release_blocking_torque_on_ = false;
        last_torque_on_cancellation_requested_ = false;
    }

    void failNextTorqueOnCancellation()
    {
        std::lock_guard lock(motion_mutex_);
        fail_next_torque_on_cancellation_ = true;
    }

    bool waitForBlockingTorqueOn(
        const std::chrono::milliseconds timeout)
    {
        std::unique_lock lock(motion_mutex_);
        return torque_on_started_cv_.wait_for(
            lock,
            timeout,
            [this]() { return blocking_torque_on_started_; });
    }

    void releaseBlockingTorqueOn()
    {
        {
            std::lock_guard lock(motion_mutex_);
            release_blocking_torque_on_ = true;
        }
        torque_on_release_cv_.notify_all();
    }

    bool waitForBlockingMotion(
        const std::string& operation,
        const std::chrono::milliseconds timeout)
    {
        std::unique_lock lock(motion_mutex_);
        return motion_started_cv_.wait_for(
            lock,
            timeout,
            [this, &operation]() {
                return blocking_motion_started_ &&
                       blocking_motion_name_ == operation;
            });
    }

    void releaseBlockingMotion()
    {
        {
            std::lock_guard lock(motion_mutex_);
            release_blocking_motion_ = true;
        }
        motion_release_cv_.notify_all();
    }

    void blockNextStopMotion()
    {
        std::lock_guard lock(motion_mutex_);
        block_next_stop_ = true;
        blocking_stop_started_ = false;
        release_blocking_stop_ = false;
    }

    void failNextStopMotion()
    {
        std::lock_guard lock(motion_mutex_);
        fail_next_stop_ = true;
    }

    void throwNextStopMotion()
    {
        std::lock_guard lock(motion_mutex_);
        throw_next_stop_ = true;
    }

    bool waitForBlockingStop(const std::chrono::milliseconds timeout)
    {
        std::unique_lock lock(motion_mutex_);
        return stop_started_cv_.wait_for(
            lock,
            timeout,
            [this]() { return blocking_stop_started_; });
    }

    void releaseBlockingStop()
    {
        {
            std::lock_guard lock(motion_mutex_);
            release_blocking_stop_ = true;
        }
        stop_release_cv_.notify_all();
    }

    int moveJCalls() const
    {
        std::lock_guard lock(motion_mutex_);
        return move_j_calls_;
    }

    int moveLCalls() const
    {
        std::lock_guard lock(motion_mutex_);
        return move_l_calls_;
    }

    int stopMotionCalls() const
    {
        std::lock_guard lock(motion_mutex_);
        return stop_motion_calls_;
    }

    int torqueOffCalls() const
    {
        std::lock_guard lock(motion_mutex_);
        return torque_off_calls_;
    }

    int torqueOnCalls() const
    {
        std::lock_guard lock(motion_mutex_);
        return torque_on_calls_;
    }

    bool lastTorqueOnHadCancellation() const
    {
        std::lock_guard lock(motion_mutex_);
        return last_torque_on_had_cancellation_;
    }

    bool lastTorqueOnCancellationRequested() const
    {
        std::lock_guard lock(motion_mutex_);
        return last_torque_on_cancellation_requested_;
    }

    bool lastMotionHadCancellation() const
    {
        std::lock_guard lock(motion_mutex_);
        return last_motion_had_cancellation_;
    }

    bool lastMotionCancellationRequested() const
    {
        std::lock_guard lock(motion_mutex_);
        return last_motion_cancellation_requested_;
    }

    device::Result startServoMode(const device::ServoOptions&) override
    {
        return device::Result::success();
    }
    device::Result servoJ(const device::JointPositionCommand&) override
    {
        return device::Result::success();
    }
    device::Result servoL(
        const device::CartesianPose&,
        device::FrameType = device::FrameType::Base) override
    {
        return device::Result::success();
    }
    device::Result servoSpeedJ(const device::JointVelocityCommand&) override
    {
        return device::Result::success();
    }
    device::Result servoSpeedL(
        const device::CartesianVelocity&,
        device::FrameType = device::FrameType::Base) override
    {
        return device::Result::success();
    }
    device::Result stopServoMode() override
    {
        return device::Result::success();
    }

    device::Result connect(const std::string&, int) override
    {
        return device::Result::success();
    }
    device::Result disconnect() override
    {
        return device::Result::success();
    }
    bool isConnected() const override { return true; }
    device::Result powerOn() override { return device::Result::success(); }
    device::Result powerOff() override { return device::Result::success(); }
    device::Result brakeRelease() override
    {
        return device::Result::success();
    }
    device::Result shutdown() override
    {
        return device::Result::success();
    }
    device::Result clearFault() override
    {
        return device::Result::success();
    }
    device::Result unlockProtectiveStop() override
    {
        return device::Result::success();
    }
    device::Result loadProgram(const std::string&) override
    {
        return device::Result::success();
    }
    device::Result playProgram() override
    {
        return device::Result::success();
    }
    device::Result pauseProgram() override
    {
        return device::Result::success();
    }
    device::Result stopProgram() override
    {
        return device::Result::success();
    }

    std::vector<double> ik(const std::string&,
                           const std::string&,
                           const device::CartesianPose&) override
    {
        return {};
    }
    std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override
    {
        return nullptr;
    }
    device::CartesianPose fk(const std::string&,
                             const std::string&) override
    {
        return {};
    }
    device::CartesianPose fk(bool = true) override { return {}; }
    device::CartesianVelocity getSpeedLCommandTwistBase() const override
    {
        return {};
    }
    bool busy() const override { return false; }

    int execute_calls{0};
    bool next_success{true};
    std::string next_response_json;
    std::string last_request_json;

private:
    device::Result enterMotion(
        const char* operation,
        int& call_count,
        const device::MotionOptions& options)
    {
        std::unique_lock lock(motion_mutex_);
        ++call_count;
        last_motion_had_cancellation_ =
            static_cast<bool>(options.cancellation_requested);
        last_motion_cancellation_requested_ =
            last_motion_had_cancellation_ &&
            options.cancellation_requested();
        if (!block_next_motion_) {
            return device::Result::success();
        }

        block_next_motion_ = false;
        blocking_motion_started_ = true;
        blocking_motion_name_ = operation;
        motion_started_cv_.notify_all();
        motion_release_cv_.wait(
            lock,
            [this]() { return release_blocking_motion_; });
        return device::Result::success();
    }

    mutable std::mutex motion_mutex_;
    std::condition_variable motion_started_cv_;
    std::condition_variable motion_release_cv_;
    std::condition_variable stop_started_cv_;
    std::condition_variable stop_release_cv_;
    std::condition_variable torque_on_started_cv_;
    std::condition_variable torque_on_release_cv_;
    bool block_next_motion_{false};
    bool blocking_motion_started_{false};
    bool release_blocking_motion_{false};
    bool block_next_stop_{false};
    bool blocking_stop_started_{false};
    bool release_blocking_stop_{false};
    bool fail_next_stop_{false};
    bool throw_next_stop_{false};
    bool block_next_torque_on_{false};
    bool blocking_torque_on_started_{false};
    bool release_blocking_torque_on_{false};
    bool fail_next_torque_on_cancellation_{false};
    std::string blocking_motion_name_;
    int move_j_calls_{0};
    int move_l_calls_{0};
    int stop_motion_calls_{0};
    int torque_off_calls_{0};
    int torque_on_calls_{0};
    bool last_torque_on_had_cancellation_{false};
    bool last_torque_on_cancellation_requested_{false};
    bool last_motion_had_cancellation_{false};
    bool last_motion_cancellation_requested_{false};
};

class JsonCommandNonArmDevice final : public device::AbstractDevice {
public:
    explicit JsonCommandNonArmDevice(std::string id)
        : AbstractDevice(std::move(id))
    {
    }

    std::string typeName() const override { return "JsonCommandNonArmDevice"; }

    bool executeJsonCommand(const std::string&,
                            std::string& response_json) override
    {
        ++execute_calls;
        response_json = R"({"success":true})";
        return true;
    }

    int execute_calls{0};
};

class GrpcArmServiceTest : public ::testing::Test {
protected:
    void SetUp() override
    {
        control::ControlAuthorityManager::instance().clear();
        globalStopAllAdmissionGate().clearForTesting();
        device::DeviceManager::destroyInstance();
        config::DeviceManagerConfig config;
        auto& manager = device::DeviceManager::getInstance(config);

        left_arm_ = std::make_shared<JsonCommandRobotArm>("left_arm");
        aubo_arm_ = std::make_shared<JsonCommandRobotArm>("aubo_arm");
        non_arm_ = std::make_shared<JsonCommandNonArmDevice>("camera");
        manager.registerDevice(left_arm_);
        manager.registerDevice(aubo_arm_);
        manager.registerDevice(non_arm_);
        service_ = std::make_unique<gRPCArmServiceImpl>();
    }

    void TearDown() override
    {
        service_.reset();
        non_arm_.reset();
        aubo_arm_.reset();
        left_arm_.reset();
        device::DeviceManager::destroyInstance();
        control::ControlAuthorityManager::instance().clear();
        globalStopAllAdmissionGate().clearForTesting();
    }

    grpc::Status execute(const std::string& device_id,
                         const std::string& request_json,
                         api::JsonDeviceCommand_Feedback& response)
    {
        api::JsonDeviceCommand_Request request;
        request.mutable_header()->set_device_id(device_id);
        request.set_request_json(request_json);
        grpc::ServerContext context;
        return service_->ExecuteJsonCommand(&context, &request, &response);
    }

    struct MoveOutcome {
        grpc::Status status;
        bool response_success{false};
        std::string response_error;
    };

    MoveOutcome moveJ(const std::string& device_id)
    {
        api::MoveJ_Request request;
        request.mutable_header()->set_device_id(device_id);
        request.mutable_target()->add_position(0.1);
        api::MoveJ_Response response;
        grpc::ServerContext context;
        auto status = service_->moveJ(&context, &request, &response);
        return {
            std::move(status),
            response.header().success(),
            response.header().error_message()};
    }

    grpc::Status identifiedMoveJ(
        const std::string& command_id,
        const double position,
        api::MoveJ_Response& response)
    {
        api::MoveJ_Request request;
        auto* header = request.mutable_header();
        header->set_device_id("aubo_arm");
        header->set_command_id(command_id);
        header->set_expected_service_instance_id(
            device::DeviceManager::getInstance()
                .safetyCoordinator()
                .serviceInstanceId());
        header->set_valid_for_ms(1000);
        request.mutable_target()->add_position(position);
        grpc::ServerContext context;
        return service_->moveJ(&context, &request, &response);
    }

    MoveOutcome moveL(const std::string& device_id)
    {
        api::MoveL_Request request;
        request.mutable_header()->set_device_id(device_id);
        request.mutable_target()->set_x(0.1);
        api::MoveL_Response response;
        grpc::ServerContext context;
        auto status = service_->moveL(&context, &request, &response);
        return {
            std::move(status),
            response.header().success(),
            response.header().error_message()};
    }

    grpc::Status stopMotion(
        const std::string& device_id,
        api::CommandHeader_Feedback& response)
    {
        api::CommandHeader_Request request;
        request.set_device_id(device_id);
        grpc::ServerContext context;
        return service_->stopMotion(&context, &request, &response);
    }

    grpc::Status torqueOff(
        const std::string& device_id,
        api::CommandHeader_Feedback& response)
    {
        api::CommandHeader_Request request;
        request.set_device_id(device_id);
        grpc::ServerContext context;
        return service_->torqueOff(&context, &request, &response);
    }

    MoveOutcome torqueOn(const std::string& device_id)
    {
        api::CommandHeader_Request request;
        request.set_device_id(device_id);
        api::CommandHeader_Feedback response;
        grpc::ServerContext context;
        auto status = service_->torqueOn(&context, &request, &response);
        return {
            std::move(status),
            response.success(),
            response.error_message()};
    }

    std::shared_ptr<JsonCommandRobotArm> left_arm_;
    std::shared_ptr<JsonCommandRobotArm> aubo_arm_;
    std::shared_ptr<JsonCommandNonArmDevice> non_arm_;
    std::unique_ptr<gRPCArmServiceImpl> service_;
};

TEST(GrpcArmServiceDescriptorTest,
     ExecuteJsonCommandBelongsOnlyToArmService)
{
    const auto* pool = google::protobuf::DescriptorPool::generated_pool();
    const auto* arm_service =
        pool->FindServiceByName("cmvr.api.ArmService");
    const auto* system_service =
        pool->FindServiceByName("cmvr.api.SystemService");

    ASSERT_NE(arm_service, nullptr);
    ASSERT_NE(system_service, nullptr);
    EXPECT_NE(arm_service->FindMethodByName("ExecuteJsonCommand"), nullptr);
    EXPECT_EQ(system_service->FindMethodByName("ExecuteJsonCommand"), nullptr);
}

TEST_F(GrpcArmServiceTest, RoutesByHeaderDeviceIdAndForwardsSuccessfulJson)
{
    const std::string request_json =
        R"({"command":"cabinet_io","operation":"get_di","index":0})";
    const std::string response_json =
        R"({"success":true,"operation":"get_di","index":0,"value":false})";
    aubo_arm_->next_response_json = response_json;

    api::JsonDeviceCommand_Feedback response;
    const auto status = execute("aubo_arm", request_json, response);

    ASSERT_TRUE(status.ok()) << status.error_message();
    EXPECT_TRUE(response.header().success());
    EXPECT_TRUE(response.header().error_message().empty());
    EXPECT_TRUE(response.header().has_timestamp());
    EXPECT_GT(response.header().timestamp().seconds(), 0);
    EXPECT_EQ(response.response_json(), response_json);
    EXPECT_EQ(aubo_arm_->execute_calls, 1);
    EXPECT_EQ(aubo_arm_->last_request_json, request_json);
    EXPECT_EQ(left_arm_->execute_calls, 0);
}

TEST_F(GrpcArmServiceTest, ForwardsDeviceJsonFailureWithLegacyGrpcOkSemantics)
{
    const std::string response_json =
        R"({"success":false,"error_code":"not_connected"})";
    aubo_arm_->next_success = false;
    aubo_arm_->next_response_json = response_json;

    api::JsonDeviceCommand_Feedback response;
    const auto status = execute(
        "aubo_arm",
        R"({"command":"cabinet_io","operation":"get_do","index":0})",
        response);

    ASSERT_TRUE(status.ok()) << status.error_message();
    EXPECT_FALSE(response.header().success());
    EXPECT_EQ(response.header().error_message(), response_json);
    EXPECT_TRUE(response.header().has_timestamp());
    EXPECT_GT(response.header().timestamp().seconds(), 0);
    EXPECT_EQ(response.response_json(), response_json);
    EXPECT_EQ(aubo_arm_->execute_calls, 1);
    EXPECT_EQ(left_arm_->execute_calls, 0);
}

TEST_F(GrpcArmServiceTest,
       MissingOrNonArmIdReturnsBusinessFailureWithoutBackendDispatch)
{
    api::JsonDeviceCommand_Feedback non_arm_response;
    const auto non_arm_status = execute(
        "camera", R"({"command":"cabinet_io"})", non_arm_response);

    ASSERT_TRUE(non_arm_status.ok()) << non_arm_status.error_message();
    EXPECT_FALSE(non_arm_response.header().success());
    EXPECT_EQ(non_arm_response.header().error_message(),
              "Device not found: camera");
    EXPECT_TRUE(non_arm_response.header().has_timestamp());
    EXPECT_TRUE(non_arm_response.response_json().empty());
    EXPECT_EQ(non_arm_->execute_calls, 0);
    EXPECT_EQ(aubo_arm_->execute_calls, 0);
    EXPECT_EQ(left_arm_->execute_calls, 0);

    api::JsonDeviceCommand_Feedback missing_response;
    const auto missing_status = execute(
        "missing_arm", R"({"command":"cabinet_io"})", missing_response);

    ASSERT_TRUE(missing_status.ok()) << missing_status.error_message();
    EXPECT_FALSE(missing_response.header().success());
    EXPECT_EQ(missing_response.header().error_message(),
              "Device not found: missing_arm");
    EXPECT_TRUE(missing_response.header().has_timestamp());
    EXPECT_TRUE(missing_response.response_json().empty());
    EXPECT_EQ(non_arm_->execute_calls, 0);
    EXPECT_EQ(aubo_arm_->execute_calls, 0);
    EXPECT_EQ(left_arm_->execute_calls, 0);
}

TEST_F(GrpcArmServiceTest,
       StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns)
{
    aubo_arm_->blockNextMotion();
    auto blocked_move = std::async(
        std::launch::async,
        [this]() { return moveJ("aubo_arm"); });

    const bool move_started = aubo_arm_->waitForBlockingMotion(
        "moveJ", std::chrono::seconds(2));

    MoveOutcome conflict;
    MoveOutcome during_stop;
    api::CommandHeader_Feedback torque_off_response;
    grpc::Status torque_off_status;
    api::CommandHeader_Feedback stop_response;
    grpc::Status stop_status;
    MoveOutcome before_retired_handler_release;
    bool stop_started = false;
    std::future<grpc::Status> blocked_stop;
    std::future<grpc::Status> torque_off;
    if (move_started) {
        conflict = moveL("aubo_arm");
        aubo_arm_->blockNextStopMotion();
        blocked_stop = std::async(
            std::launch::async,
            [this, &stop_response]() {
                return stopMotion("aubo_arm", stop_response);
            });
        stop_started = aubo_arm_->waitForBlockingStop(
            std::chrono::seconds(2));
        if (stop_started) {
            torque_off = std::async(
                std::launch::async,
                [this, &torque_off_response]() {
                    return torqueOff("aubo_arm", torque_off_response);
                });
            during_stop = moveL("aubo_arm");
        }
        aubo_arm_->releaseBlockingStop();
        before_retired_handler_release = moveL("aubo_arm");
    }

    // Keep the original RPC active until after the replacement MoveL has
    // attempted to acquire control. This models a driver whose stopped motion
    // takes time to unwind and guards the lease hand-off itself.
    aubo_arm_->releaseBlockingMotion();
    const auto original_move = blocked_move.get();
    if (blocked_stop.valid()) {
        stop_status = blocked_stop.get();
    }
    if (torque_off.valid()) {
        torque_off_status = torque_off.get();
    }
    const auto resumed_move = moveL("aubo_arm");

    ASSERT_TRUE(move_started);
    ASSERT_TRUE(stop_started);
    EXPECT_EQ(conflict.status.error_code(),
              grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(conflict.response_error, conflict.status.error_message());
    EXPECT_EQ(during_stop.status.error_code(),
              grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(during_stop.response_error,
              during_stop.status.error_message());
    EXPECT_TRUE(torque_off_status.ok())
        << torque_off_status.error_message();
    EXPECT_TRUE(torque_off_response.success())
        << torque_off_response.error_message();
    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
    EXPECT_TRUE(stop_response.success())
        << stop_response.error_message();
    EXPECT_EQ(
        before_retired_handler_release.status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_TRUE(resumed_move.status.ok())
        << resumed_move.status.error_message();
    EXPECT_TRUE(resumed_move.response_success)
        << resumed_move.response_error;
    EXPECT_TRUE(original_move.status.ok())
        << original_move.status.error_message();
    EXPECT_TRUE(original_move.response_success)
        << original_move.response_error;
    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
    EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);
    EXPECT_EQ(aubo_arm_->torqueOffCalls(), 2);
}

TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation)
{
    const auto outcome = moveJ("aubo_arm");

    ASSERT_TRUE(outcome.status.ok())
        << outcome.status.error_message();
    EXPECT_TRUE(outcome.response_success) << outcome.response_error;
    EXPECT_TRUE(aubo_arm_->lastMotionHadCancellation());
    EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested());
}

TEST_F(GrpcArmServiceTest,
       IdenticalCommandIdReplaysCachedResultWithoutRedispatch)
{
    api::MoveJ_Response first;
    api::MoveJ_Response retry;

    const auto first_status = identifiedMoveJ(
        "arm-movej-idempotency-1", 0.1, first);
    const auto retry_status = identifiedMoveJ(
        "arm-movej-idempotency-1", 0.1, retry);

    ASSERT_TRUE(first_status.ok()) << first_status.error_message();
    ASSERT_TRUE(retry_status.ok()) << retry_status.error_message();
    EXPECT_TRUE(first.header().success());
    EXPECT_TRUE(retry.header().success());
    EXPECT_EQ(
        retry.header().execution_state(),
        api::COMMAND_EXECUTION_STATE_COMPLETED);
    EXPECT_EQ(retry.header().command_id(), "arm-movej-idempotency-1");
    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
}

TEST_F(GrpcArmServiceTest,
       ReusedCommandIdWithDifferentPayloadIsRejectedWithoutRedispatch)
{
    api::MoveJ_Response first;
    api::MoveJ_Response conflict;

    const auto first_status = identifiedMoveJ(
        "arm-movej-conflict-1", 0.1, first);
    const auto conflict_status = identifiedMoveJ(
        "arm-movej-conflict-1", 0.2, conflict);

    ASSERT_TRUE(first_status.ok()) << first_status.error_message();
    EXPECT_EQ(
        conflict_status.error_code(), grpc::StatusCode::ALREADY_EXISTS);
    EXPECT_FALSE(conflict.header().success());
    EXPECT_EQ(
        conflict.header().reason_code(),
        api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT);
    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
}

TEST_F(GrpcArmServiceTest,
       StopAllCancelsInFlightTorqueOnWithoutReportingSuccess)
{
    aubo_arm_->blockNextTorqueOn();
    auto blocked_torque_on = std::async(
        std::launch::async,
        [this]() { return torqueOn("aubo_arm"); });

    const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
        std::chrono::seconds(2));
    auto& admission = globalStopAllAdmissionGate();
    StopAllAdmissionGate::StopAllTicket stop_all_ticket;
    if (torque_on_started) {
        stop_all_ticket = admission.beginStopAll();
    }

    const bool cancelled_promptly =
        blocked_torque_on.wait_for(std::chrono::seconds(2)) ==
        std::future_status::ready;
    if (!cancelled_promptly) {
        aubo_arm_->releaseBlockingTorqueOn();
    }
    const auto outcome = blocked_torque_on.get();

    ASSERT_TRUE(torque_on_started);
    ASSERT_TRUE(stop_all_ticket.valid());
    EXPECT_TRUE(cancelled_promptly);
    EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::UNAVAILABLE);
    EXPECT_FALSE(outcome.response_success);
    EXPECT_EQ(outcome.response_error, outcome.status.error_message());
    EXPECT_NE(outcome.response_error.find("StopAll"), std::string::npos);
    EXPECT_EQ(aubo_arm_->torqueOnCalls(), 1);
    EXPECT_TRUE(aubo_arm_->lastTorqueOnHadCancellation());
    EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());

    EXPECT_TRUE(admission.finishStopAll(stop_all_ticket, true));
}

TEST_F(GrpcArmServiceTest,
       StopAllPreservesTorqueOnSafetyTerminationFailure)
{
    aubo_arm_->blockNextTorqueOn();
    aubo_arm_->failNextTorqueOnCancellation();
    auto blocked_torque_on = std::async(
        std::launch::async,
        [this]() { return torqueOn("aubo_arm"); });

    const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
        std::chrono::seconds(2));
    auto& admission = globalStopAllAdmissionGate();
    StopAllAdmissionGate::StopAllTicket stop_all_ticket;
    if (torque_on_started) {
        stop_all_ticket = admission.beginStopAll();
    }

    const bool completed_promptly =
        blocked_torque_on.wait_for(std::chrono::seconds(2)) ==
        std::future_status::ready;
    if (!completed_promptly) {
        aubo_arm_->releaseBlockingTorqueOn();
    }
    const auto outcome = blocked_torque_on.get();

    ASSERT_TRUE(torque_on_started);
    ASSERT_TRUE(stop_all_ticket.valid());
    EXPECT_TRUE(completed_promptly);
    EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_FALSE(outcome.response_success);
    EXPECT_EQ(
        outcome.response_error,
        "simulated torqueOn safety termination failure");
    EXPECT_EQ(outcome.status.error_message(), outcome.response_error);
    EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());

    EXPECT_FALSE(admission.finishStopAll(stop_all_ticket, false));
}

TEST_F(GrpcArmServiceTest,
       ClientCancellationPreservesTorqueOnSafetyTerminationFailure)
{
    std::mutex records_mutex;
    std::condition_variable records_changed;
    std::vector<GrpcFailureRecord> records;

    grpc::ServerBuilder builder;
    const std::string socket_path =
        "/tmp/cmvr_grpc_arm_service_test_" +
        std::to_string(static_cast<long long>(::getpid())) + ".sock";
    std::remove(socket_path.c_str());
    const std::string address = "unix:" + socket_path;
    builder.AddListeningPort(
        address,
        grpc::InsecureServerCredentials());
    builder.RegisterService(service_.get());
    std::vector<std::unique_ptr<
        grpc::experimental::ServerInterceptorFactoryInterface>> factories;
    factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory(
        [&records, &records_mutex, &records_changed](
            const GrpcFailureRecord& record) {
            {
                std::lock_guard lock(records_mutex);
                records.push_back(record);
            }
            records_changed.notify_all();
        }));
    builder.experimental().SetInterceptorCreators(std::move(factories));
    auto server = builder.BuildAndStart();
    ASSERT_NE(server, nullptr);

    const auto channel = grpc::CreateChannel(
        address,
        grpc::InsecureChannelCredentials());
    auto stub = api::ArmService::NewStub(channel);
    grpc::ClientContext client_context;
    client_context.set_deadline(
        std::chrono::system_clock::now() + std::chrono::seconds(3));
    api::CommandHeader_Request request;
    request.set_device_id("aubo_arm");
    api::CommandHeader_Feedback response;
    grpc::Status client_status;

    aubo_arm_->blockNextTorqueOn();
    aubo_arm_->failNextTorqueOnCancellation();
    std::thread client_call([&]() {
        client_status = stub->torqueOn(
            &client_context, request, &response);
    });
    const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn(
        std::chrono::seconds(2));
    client_context.TryCancel();
    client_call.join();

    bool failure_recorded = false;
    {
        std::unique_lock lock(records_mutex);
        failure_recorded = records_changed.wait_for(
            lock,
            std::chrono::seconds(2),
            [&records]() { return !records.empty(); });
    }
    if (!failure_recorded) {
        aubo_arm_->releaseBlockingTorqueOn();
    }
    server->Shutdown();
    server->Wait();
    std::remove(socket_path.c_str());

    ASSERT_TRUE(torque_on_started);
    EXPECT_EQ(client_status.error_code(), grpc::StatusCode::CANCELLED);
    ASSERT_TRUE(failure_recorded);
    ASSERT_EQ(records.size(), 1U);
    EXPECT_EQ(records.front().kind, GrpcFailureKind::GRPC_STATUS);
    EXPECT_EQ(records.front().status_code, grpc::StatusCode::INTERNAL);
    EXPECT_EQ(records.front().severity, GrpcFailureSeverity::ERROR);
    EXPECT_EQ(
        records.front().detail,
        "simulated torqueOn safety termination failure");
    EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested());
}

TEST_F(GrpcArmServiceTest,
       StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops)
{
    auto& admission = globalStopAllAdmissionGate();
    const auto ticket = admission.beginStopAll();
    ASSERT_TRUE(ticket.valid());

    const auto rejected_move = moveJ("aubo_arm");

    api::JsonDeviceCommand_Feedback json_response;
    const auto json_status = execute(
        "aubo_arm", R"({"command":"cabinet_io"})", json_response);

    api::JointRequest state_request;
    state_request.mutable_header()->set_device_id("aubo_arm");
    api::JointResponse state_response;
    grpc::ServerContext state_context;
    const auto state_status = service_->getJointState(
        &state_context, &state_request, &state_response);

    api::CommandHeader_Feedback stop_response;
    const auto stop_status = stopMotion("aubo_arm", stop_response);

    EXPECT_EQ(
        rejected_move.status.error_code(),
        grpc::StatusCode::UNAVAILABLE);
    EXPECT_FALSE(rejected_move.response_success);
    EXPECT_EQ(aubo_arm_->moveJCalls(), 0);
    EXPECT_EQ(json_status.error_code(), grpc::StatusCode::UNAVAILABLE);
    EXPECT_FALSE(json_response.header().success());
    EXPECT_EQ(aubo_arm_->execute_calls, 0);

    EXPECT_TRUE(state_status.ok()) << state_status.error_message();
    EXPECT_TRUE(state_response.header().success());
    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
    EXPECT_TRUE(stop_response.success())
        << stop_response.error_message();
    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);

    EXPECT_TRUE(admission.finishStopAll(ticket, true));
    const auto resumed_move = moveJ("aubo_arm");
    EXPECT_TRUE(resumed_move.status.ok())
        << resumed_move.status.error_message();
    EXPECT_TRUE(resumed_move.response_success)
        << resumed_move.response_error;
    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
}

TEST_F(GrpcArmServiceTest,
       StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns)
{
    aubo_arm_->blockNextMotion();
    auto blocked_move = std::async(
        std::launch::async,
        [this]() { return moveL("aubo_arm"); });

    const bool move_started = aubo_arm_->waitForBlockingMotion(
        "moveL", std::chrono::seconds(2));

    MoveOutcome conflict;
    api::CommandHeader_Feedback stop_response;
    grpc::Status stop_status;
    MoveOutcome before_retired_handler_release;
    if (move_started) {
        conflict = moveJ("aubo_arm");
        auto stop = std::async(
            std::launch::async,
            [this, &stop_response]() {
                return stopMotion("aubo_arm", stop_response);
            });
        before_retired_handler_release = moveJ("aubo_arm");
        aubo_arm_->releaseBlockingMotion();
        stop_status = stop.get();
    }

    aubo_arm_->releaseBlockingMotion();
    const auto original_move = blocked_move.get();
    const auto resumed_move = moveJ("aubo_arm");

    ASSERT_TRUE(move_started);
    EXPECT_EQ(conflict.status.error_code(),
              grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(conflict.response_error, conflict.status.error_message());
    EXPECT_TRUE(stop_status.ok()) << stop_status.error_message();
    EXPECT_TRUE(stop_response.success())
        << stop_response.error_message();
    EXPECT_EQ(
        before_retired_handler_release.status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_TRUE(resumed_move.status.ok())
        << resumed_move.status.error_message();
    EXPECT_TRUE(resumed_move.response_success)
        << resumed_move.response_error;
    EXPECT_TRUE(original_move.status.ok())
        << original_move.status.error_message();
    EXPECT_TRUE(original_move.response_success)
        << original_move.response_error;
    EXPECT_EQ(aubo_arm_->moveJCalls(), 1);
    EXPECT_EQ(aubo_arm_->moveLCalls(), 1);
    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2);
}

TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier)
{
    auto& authority = control::ControlAuthorityManager::instance();
    const auto action_lease = authority.tryAcquire(
        "aubo_arm", "action-queue:test", std::chrono::hours(1));
    ASSERT_TRUE(action_lease.acquired) << action_lease.detail;
    aubo_arm_->failNextStopMotion();

    api::CommandHeader_Feedback stop_response;
    const auto stop_status = stopMotion("aubo_arm", stop_response);
    const auto rejected_move = moveJ("aubo_arm");

    EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_FALSE(stop_response.success());
    EXPECT_EQ(
        stop_response.reason_code(),
        api::COMMAND_REASON_CODE_STOP_UNCONFIRMED);
    EXPECT_FALSE(authority.validate(action_lease.token));
    EXPECT_TRUE(authority.isLeased("aubo_arm"));
    EXPECT_EQ(
        rejected_move.status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(aubo_arm_->moveJCalls(), 0);
    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);

    authority.release(action_lease.token);
    const auto recovery = authority.preemptAcquire(
        "aubo_arm", "confirmed-stop-recovery", std::chrono::hours(1));
    ASSERT_TRUE(recovery.acquired) << recovery.detail;
    ASSERT_TRUE(authority.waitForPreemptedRelease(
        recovery.token, std::chrono::milliseconds::zero()));
    ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token));
    authority.release(recovery.token);
    const auto recovered = authority.tryAcquire(
        "aubo_arm", "move-after-recovery", std::chrono::hours(1));
    EXPECT_TRUE(recovered.acquired) << recovered.detail;
    authority.release(recovered.token);
}

TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier)
{
    auto& authority = control::ControlAuthorityManager::instance();
    const auto action_lease = authority.tryAcquire(
        "aubo_arm", "action-queue:test", std::chrono::hours(1));
    ASSERT_TRUE(action_lease.acquired) << action_lease.detail;
    aubo_arm_->throwNextStopMotion();

    api::CommandHeader_Feedback stop_response;
    const auto stop_status = stopMotion("aubo_arm", stop_response);
    const auto rejected_move = moveL("aubo_arm");

    EXPECT_EQ(
        stop_status.error_code(), grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_FALSE(stop_response.success());
    EXPECT_EQ(
        stop_response.reason_code(),
        api::COMMAND_REASON_CODE_STOP_UNCONFIRMED);
    EXPECT_FALSE(authority.validate(action_lease.token));
    EXPECT_TRUE(authority.isLeased("aubo_arm"));
    EXPECT_EQ(
        rejected_move.status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(aubo_arm_->moveLCalls(), 0);
    EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1);

    authority.release(action_lease.token);
    const auto recovery = authority.preemptAcquire(
        "aubo_arm", "confirmed-exception-recovery", std::chrono::hours(1));
    ASSERT_TRUE(recovery.acquired) << recovery.detail;
    ASSERT_TRUE(authority.waitForPreemptedRelease(
        recovery.token, std::chrono::milliseconds::zero()));
    ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token));
    authority.release(recovery.token);
}

} // namespace
} // namespace cmvr::service