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

#include "service/grpc/server/include/grpc_agv_service.h"

#include <chrono>
#include <future>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include <grpcpp/grpcpp.h>
#include <gtest/gtest.h>

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

namespace cmvr::service {
namespace {

constexpr char kNativeErrorMessage[] =
    "SEER Robokit command failed: ret_code=41200, err_msg=speed_illegal";
constexpr char kNativeNavigationErrorMessage[] =
    "SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose";

class FakeAgv final : public device::AbstractAGV {
public:
    FakeAgv()
    {
        id_ = "test-agv";
    }

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

    device::AgvResult emergencyStop() override
    {
        ++emergency_stop_calls_;
        emergency_stop_barrier_observed_ = normalLeaseIsBlocked_(
            "test-probe:emergencyStop");
        return device::AgvResult::success();
    }

    device::AgvResult navigateToPose(
        const math::Pose2d& pose,
        const device::AgvMotionOptions& options,
        const device::AgvAdapterParams&) override
    {
        ++navigate_pose_calls_;
        pose_cancellation_bound_ =
            static_cast<bool>(options.cancellation_requested);
        pose_cancellation_requested_during_call_ =
            pose_cancellation_bound_ && options.cancellation_requested();
        if (pose_cancellation_requested_during_call_) {
            return device::AgvResult::failure(
                device::AgvErrorCode::TaskCanceled,
                "navigation canceled before fake device dispatch");
        }
        pose_ = pose;
        pose_options_ = options;
        pose_options_.cancellation_requested = {};
        return pose_result_;
    }

    device::AgvResult navigateToStation(
        const std::string& station_id,
        const device::AgvMotionOptions& options,
        const device::AgvAdapterParams& adapter_params) override
    {
        station_id_ = station_id;
        station_options_ = options;
        station_adapter_params_ = adapter_params;
        station_cancellation_bound_ =
            static_cast<bool>(options.cancellation_requested);
        station_cancellation_requested_during_call_ =
            station_cancellation_bound_ && options.cancellation_requested();
        station_options_.cancellation_requested = {};
        return device::AgvResult::success();
    }

    device::AgvResult followPath(
        const std::vector<device::AgvPathSegment>& path,
        const device::AgvMotionOptions& options) override
    {
        path_ = path;
        path_options_ = options;
        path_cancellation_bound_ =
            static_cast<bool>(options.cancellation_requested);
        path_cancellation_requested_during_call_ =
            path_cancellation_bound_ && options.cancellation_requested();
        path_options_.cancellation_requested = {};
        return device::AgvResult::success();
    }

    device::AgvResult setVelocity(const device::AgvVelocity&) override
    {
        ++set_velocity_calls_;
        return device::AgvResult::failure(
            device::AgvErrorCode::CommandFailed,
            kNativeErrorMessage);
    }

    device::AgvResult cancelNavigation() override
    {
        ++cancel_navigation_calls_;
        cancel_navigation_barrier_observed_ = normalLeaseIsBlocked_(
            "test-probe:cancelNavigation");
        return device::AgvResult::success();
    }

    device::AgvResult stopVelocityControl() override
    {
        ++stop_velocity_calls_;
        stop_velocity_barrier_observed_ = normalLeaseIsBlocked_(
            "test-probe:stopVelocityControl");
        return device::AgvResult::success();
    }

    device::AgvResult confirmMotionStopped() override
    {
        ++confirm_stopped_calls_;
        confirm_stopped_barrier_observed_ = normalLeaseIsBlocked_(
            "test-probe:confirmMotionStopped");
        return confirm_stopped_result_;
    }

    bool normalLeaseIsBlocked_(const std::string& owner)
    {
        auto& authority = control::ControlAuthorityManager::instance();
        const auto probe = authority.tryAcquire(
            id_, owner, std::chrono::hours(1));
        if (probe.acquired) {
            authority.release(probe.token);
        }
        return !probe.acquired;
    }

    math::Pose2d pose_{};
    device::AgvMotionOptions pose_options_;
    device::AgvResult pose_result_{device::AgvResult::success()};
    std::string station_id_;
    device::AgvMotionOptions station_options_;
    device::AgvAdapterParams station_adapter_params_;
    std::vector<device::AgvPathSegment> path_;
    device::AgvMotionOptions path_options_;
    bool pose_cancellation_bound_{false};
    bool pose_cancellation_requested_during_call_{false};
    bool station_cancellation_bound_{false};
    bool station_cancellation_requested_during_call_{false};
    bool path_cancellation_bound_{false};
    bool path_cancellation_requested_during_call_{false};
    int emergency_stop_calls_{0};
    int cancel_navigation_calls_{0};
    int stop_velocity_calls_{0};
    int confirm_stopped_calls_{0};
    int set_velocity_calls_{0};
    int navigate_pose_calls_{0};
    bool emergency_stop_barrier_observed_{false};
    bool cancel_navigation_barrier_observed_{false};
    bool stop_velocity_barrier_observed_{false};
    bool confirm_stopped_barrier_observed_{false};
    device::AgvResult confirm_stopped_result_{
        device::AgvResult::success()};
};

class LegacyFollowPathAgv final : public device::AbstractAGV {
public:
    std::string typeName() const override { return "LegacyFollowPathAgv"; }

    device::AgvResult followPath(
        const std::vector<device::AgvPathSegment>& path) override
    {
        path_ = path;
        return device::AgvResult::success();
    }

    std::vector<device::AgvPathSegment> path_;
};

class GrpcAgvServiceTest : public ::testing::Test {
protected:
    void SetUp() override
    {
        control::ControlAuthorityManager::instance().clear();
        globalStopAllAdmissionGate().clearForTesting();
        config::DeviceManagerConfig config;
        auto& manager = device::DeviceManager::getInstance(config);
        agv_ = std::make_shared<FakeAgv>();
        manager.registerDevice(agv_);
        service_ = std::make_unique<gRPCAgvServiceImpl>();
    }

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

    std::shared_ptr<FakeAgv> agv_;
    std::unique_ptr<gRPCAgvServiceImpl> service_;
};

void setMotionOptions(msgs::AgvMotionOptions* options)
{
    options->set_max_speed(0.4);
    options->set_max_angular_speed(0.5);
    options->set_max_acceleration(0.6);
    options->set_max_angular_acceleration(0.7);
    options->set_reach_distance(0.08);
    options->set_reach_angle(0.09);
    options->set_wait_timeout_ms(1234);
    options->set_poll_interval_ms(55);
}

void expectMotionOptions(const device::AgvMotionOptions& options)
{
    EXPECT_DOUBLE_EQ(options.max_speed, 0.4);
    EXPECT_DOUBLE_EQ(options.max_angular_speed, 0.5);
    EXPECT_DOUBLE_EQ(options.max_acceleration, 0.6);
    EXPECT_DOUBLE_EQ(options.max_angular_acceleration, 0.7);
    EXPECT_DOUBLE_EQ(options.reach_distance, 0.08);
    EXPECT_DOUBLE_EQ(options.reach_angle, 0.09);
    EXPECT_EQ(options.wait_timeout_ms, 1234);
    EXPECT_EQ(options.poll_interval_ms, 55);
    EXPECT_FALSE(options.asynchronous);
}

TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions)
{
    api::AgvNavigateToPoseCommand_Request pose_request;
    pose_request.mutable_header()->set_device_id("test-agv");
    pose_request.mutable_pose()->set_x(1.0);
    pose_request.mutable_pose()->set_y(2.0);
    pose_request.mutable_pose()->set_theta(0.5);
    setMotionOptions(pose_request.mutable_options());
    api::AgvNavigateToPoseCommand_Feedback pose_response;
    grpc::ServerContext pose_context;

    const auto pose_status = service_->navigateToPose(
        &pose_context,
        &pose_request,
        &pose_response);

    ASSERT_TRUE(pose_status.ok()) << pose_status.error_message();
    EXPECT_TRUE(pose_response.header().success());
    EXPECT_DOUBLE_EQ(agv_->pose_.x, 1.0);
    EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0);
    EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5);
    expectMotionOptions(agv_->pose_options_);
    EXPECT_TRUE(agv_->pose_cancellation_bound_);
    EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_);

    api::AgvNavigateToStationCommand_Request station_request;
    station_request.mutable_header()->set_device_id("test-agv");
    station_request.set_station_id("station-1");
    setMotionOptions(station_request.mutable_options());
    api::AgvNavigateToStationCommand_Feedback station_response;
    grpc::ServerContext station_context;

    const auto station_status = service_->navigateToStation(
        &station_context,
        &station_request,
        &station_response);

    ASSERT_TRUE(station_status.ok()) << station_status.error_message();
    EXPECT_TRUE(station_response.header().success());
    EXPECT_EQ(agv_->station_id_, "station-1");
    expectMotionOptions(agv_->station_options_);
    EXPECT_TRUE(agv_->station_cancellation_bound_);
    EXPECT_FALSE(agv_->station_cancellation_requested_during_call_);

    api::AgvFollowPathCommand_Request path_request;
    path_request.mutable_header()->set_device_id("test-agv");
    auto* segment = path_request.add_path();
    segment->set_source_station("station-1");
    segment->set_target_station("station-2");
    setMotionOptions(path_request.mutable_options());
    api::AgvFollowPathCommand_Feedback path_response;
    grpc::ServerContext path_context;

    const auto path_status = service_->followPath(
        &path_context,
        &path_request,
        &path_response);

    ASSERT_TRUE(path_status.ok()) << path_status.error_message();
    EXPECT_TRUE(path_response.header().success());
    ASSERT_EQ(agv_->path_.size(), 1U);
    EXPECT_EQ(agv_->path_[0].source_station, "station-1");
    EXPECT_EQ(agv_->path_[0].target_station, "station-2");
    expectMotionOptions(agv_->path_options_);
    EXPECT_TRUE(agv_->path_cancellation_bound_);
    EXPECT_FALSE(agv_->path_cancellation_requested_during_call_);
}

TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage)
{
    api::AgvSetVelocityCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    request.mutable_velocity()->set_vx(0.1);
    api::AgvSetVelocityCommand_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->setVelocity(
        &context,
        &request,
        &response);

    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_EQ(status.error_message(), kNativeErrorMessage);
    EXPECT_FALSE(response.header().success());
    EXPECT_EQ(response.header().error_message(), kNativeErrorMessage);
}

TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage)
{
    agv_->pose_result_ = device::AgvResult::failure(
        device::AgvErrorCode::CommandFailed,
        kNativeNavigationErrorMessage);
    api::AgvNavigateToPoseCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    request.mutable_pose()->set_x(1.0);
    request.mutable_pose()->set_y(2.0);
    api::AgvNavigateToPoseCommand_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->navigateToPose(
        &context,
        &request,
        &response);

    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_EQ(status.error_message(), kNativeNavigationErrorMessage);
    EXPECT_FALSE(response.header().success());
    EXPECT_EQ(
        response.header().error_message(),
        kNativeNavigationErrorMessage);
    EXPECT_FALSE(agv_->pose_options_.asynchronous);
    EXPECT_EQ(agv_->pose_options_.wait_timeout_ms, 0);
    EXPECT_EQ(agv_->pose_options_.poll_interval_ms, 0);
    EXPECT_TRUE(agv_->pose_cancellation_bound_);
    EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_);
}

TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded)
{
    api::AgvNavigateToStationCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    request.set_station_id("station-async");
    request.mutable_options()->set_asynchronous(true);
    api::AgvNavigateToStationCommand_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->navigateToStation(
        &context,
        &request,
        &response);

    ASSERT_TRUE(status.ok()) << status.error_message();
    EXPECT_TRUE(agv_->station_options_.asynchronous);
    EXPECT_TRUE(agv_->station_cancellation_bound_);
    EXPECT_FALSE(agv_->station_cancellation_requested_during_call_);
}

TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs)
{
    auto& authority = control::ControlAuthorityManager::instance();
    const auto action_lease = authority.tryAcquire(
        "test-agv",
        "action-sequence:test-action",
        std::chrono::hours(1));
    ASSERT_TRUE(action_lease.acquired) << action_lease.detail;

    api::AgvNavigateToPoseCommand_Request navigation_request;
    navigation_request.mutable_header()->set_device_id("test-agv");
    api::AgvNavigateToPoseCommand_Feedback navigation_response;
    grpc::ServerContext navigation_context;
    const auto navigation_status = service_->navigateToPose(
        &navigation_context,
        &navigation_request,
        &navigation_response);
    EXPECT_EQ(
        navigation_status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_FALSE(navigation_response.header().success());

    api::CommandHeader_Request clear_fault_request;
    clear_fault_request.set_device_id("test-agv");
    api::CommandHeader_Feedback clear_fault_response;
    grpc::ServerContext clear_fault_context;
    const auto clear_fault_status = service_->clearFault(
        &clear_fault_context,
        &clear_fault_request,
        &clear_fault_response);
    EXPECT_EQ(
        clear_fault_status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_FALSE(clear_fault_response.success());

    api::AgvMapCommand_Request switch_map_request;
    switch_map_request.mutable_header()->set_device_id("test-agv");
    switch_map_request.set_map_name("map-1");
    api::AgvMapCommand_Feedback switch_map_response;
    grpc::ServerContext switch_map_context;
    const auto switch_map_status = service_->switchMap(
        &switch_map_context,
        &switch_map_request,
        &switch_map_response);
    EXPECT_EQ(
        switch_map_status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_FALSE(switch_map_response.header().success());

    authority.release(action_lease.token);
}

TEST_F(GrpcAgvServiceTest, NavigationCancellationIncludesControlLease)
{
    auto& authority = control::ControlAuthorityManager::instance();
    const auto barrier = authority.preemptAcquire(
        "test-agv", "stop-all-test", std::chrono::hours(1));
    ASSERT_TRUE(barrier.acquired) << barrier.detail;

    api::AgvNavigateToPoseCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    api::AgvNavigateToPoseCommand_Feedback response;
    grpc::ServerContext context;
    const auto status = service_->navigateToPose(
        &context, &request, &response);

    EXPECT_EQ(
        status.error_code(),
        grpc::StatusCode::FAILED_PRECONDITION);
    EXPECT_EQ(agv_->navigate_pose_calls_, 0);
    authority.release(barrier.token);
}

TEST_F(GrpcAgvServiceTest, SetVelocityDispatchRunsUnderControlFence)
{
    api::AgvSetVelocityCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    request.mutable_velocity()->set_vx(0.1);
    api::AgvSetVelocityCommand_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->setVelocity(
        &context, &request, &response);

    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_EQ(agv_->set_velocity_calls_, 1);
}

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

    api::AgvNavigateToPoseCommand_Request navigation_request;
    navigation_request.mutable_header()->set_device_id("test-agv");
    api::AgvNavigateToPoseCommand_Feedback navigation_response;
    grpc::ServerContext navigation_context;
    const auto navigation_status = service_->navigateToPose(
        &navigation_context, &navigation_request, &navigation_response);

    api::AgvSetVelocityCommand_Request velocity_request;
    velocity_request.mutable_header()->set_device_id("test-agv");
    velocity_request.mutable_velocity()->set_vx(0.1);
    api::AgvSetVelocityCommand_Feedback velocity_response;
    grpc::ServerContext velocity_context;
    const auto velocity_status = service_->setVelocity(
        &velocity_context, &velocity_request, &velocity_response);

    api::AgvRuntimeStateCommand_Request state_request;
    state_request.mutable_header()->set_device_id("test-agv");
    api::AgvRuntimeStateCommand_Feedback state_response;
    grpc::ServerContext state_context;
    const auto state_status = service_->getRuntimeState(
        &state_context, &state_request, &state_response);

    api::CommandHeader_Request stop_request;
    stop_request.set_device_id("test-agv");
    api::CommandHeader_Feedback stop_response;
    grpc::ServerContext stop_context;
    const auto stop_status = service_->cancelNavigation(
        &stop_context, &stop_request, &stop_response);

    EXPECT_EQ(
        navigation_status.error_code(),
        grpc::StatusCode::UNAVAILABLE);
    EXPECT_FALSE(navigation_response.header().success());
    EXPECT_EQ(agv_->navigate_pose_calls_, 0);
    EXPECT_EQ(
        velocity_status.error_code(),
        grpc::StatusCode::UNAVAILABLE);
    EXPECT_FALSE(velocity_response.header().success());
    EXPECT_EQ(agv_->set_velocity_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(agv_->cancel_navigation_calls_, 2);

    EXPECT_TRUE(admission.finishStopAll(ticket, true));
    const auto resumed_status = service_->navigateToPose(
        &navigation_context, &navigation_request, &navigation_response);
    EXPECT_TRUE(resumed_status.ok()) << resumed_status.error_message();
    EXPECT_TRUE(navigation_response.header().success());
    EXPECT_EQ(agv_->navigate_pose_calls_, 1);
}

TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease)
{
    auto& authority = control::ControlAuthorityManager::instance();
    const auto action_lease = authority.tryAcquire(
        "test-agv",
        "action-sequence:test-action",
        std::chrono::hours(1));
    ASSERT_TRUE(action_lease.acquired) << action_lease.detail;

    api::AgvRuntimeStateCommand_Request state_request;
    state_request.mutable_header()->set_device_id("test-agv");
    api::AgvRuntimeStateCommand_Feedback state_response;
    grpc::ServerContext state_context;
    const auto state_status = service_->getRuntimeState(
        &state_context,
        &state_request,
        &state_response);
    EXPECT_TRUE(state_status.ok()) << state_status.error_message();
    EXPECT_TRUE(state_response.header().success());
    EXPECT_TRUE(authority.validate(action_lease.token));

    api::CommandHeader_Request stop_request;
    stop_request.set_device_id("test-agv");

    api::CommandHeader_Feedback emergency_response;
    grpc::ServerContext emergency_context;
    auto emergency = std::async(
        std::launch::async,
        [this, &emergency_context, &stop_request, &emergency_response]() {
            return service_->emergencyStop(
                &emergency_context,
                &stop_request,
                &emergency_response);
        });
    const auto emergency_deadline =
        std::chrono::steady_clock::now() + std::chrono::seconds(1);
    while (authority.validate(action_lease.token) &&
           std::chrono::steady_clock::now() < emergency_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    EXPECT_FALSE(authority.validate(action_lease.token));
    authority.release(action_lease.token);
    const auto emergency_status = emergency.get();
    EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message();
    EXPECT_TRUE(agv_->emergency_stop_barrier_observed_);

    const auto released_lease_after_emergency = authority.tryAcquire(
        "test-agv",
        "action-sequence:after-emergency-release",
        std::chrono::hours(1));
    ASSERT_TRUE(released_lease_after_emergency.acquired)
        << released_lease_after_emergency.detail;

    api::CommandHeader_Feedback cancel_response;
    grpc::ServerContext cancel_context;
    auto cancel = std::async(
        std::launch::async,
        [this, &cancel_context, &stop_request, &cancel_response]() {
            return service_->cancelNavigation(
                &cancel_context,
                &stop_request,
                &cancel_response);
        });
    const auto cancel_deadline =
        std::chrono::steady_clock::now() + std::chrono::seconds(1);
    while (authority.validate(released_lease_after_emergency.token) &&
           std::chrono::steady_clock::now() < cancel_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    EXPECT_FALSE(authority.validate(
        released_lease_after_emergency.token));
    authority.release(released_lease_after_emergency.token);
    const auto cancel_status = cancel.get();
    EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message();
    EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_);

    const auto released_lease_after_cancel = authority.tryAcquire(
        "test-agv",
        "action-sequence:after-cancel-release",
        std::chrono::hours(1));
    ASSERT_TRUE(released_lease_after_cancel.acquired)
        << released_lease_after_cancel.detail;

    api::CommandHeader_Feedback velocity_response;
    grpc::ServerContext velocity_context;
    auto velocity = std::async(
        std::launch::async,
        [this, &velocity_context, &stop_request, &velocity_response]() {
            return service_->stopVelocityControl(
                &velocity_context,
                &stop_request,
                &velocity_response);
        });
    const auto velocity_deadline =
        std::chrono::steady_clock::now() + std::chrono::seconds(1);
    while (authority.validate(released_lease_after_cancel.token) &&
           std::chrono::steady_clock::now() < velocity_deadline) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    EXPECT_FALSE(authority.validate(
        released_lease_after_cancel.token));
    authority.release(released_lease_after_cancel.token);
    const auto velocity_status = velocity.get();
    EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message();
    EXPECT_TRUE(agv_->stop_velocity_barrier_observed_);

    const auto lease_after_stops = authority.tryAcquire(
        "test-agv",
        "action-sequence:after-stops",
        std::chrono::hours(1));
    ASSERT_TRUE(lease_after_stops.acquired)
        << lease_after_stops.detail;

    EXPECT_EQ(agv_->emergency_stop_calls_, 2);
    EXPECT_EQ(agv_->cancel_navigation_calls_, 2);
    EXPECT_EQ(agv_->stop_velocity_calls_, 2);
    EXPECT_EQ(agv_->confirm_stopped_calls_, 3);
    EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_);

    authority.release(lease_after_stops.token);
}

TEST_F(GrpcAgvServiceTest,
       SafetyStopQuarantinesControlWhenStoppedStateIsUnconfirmed)
{
    agv_->confirm_stopped_result_ = device::AgvResult::failure(
        device::AgvErrorCode::Timeout,
        "two zero-velocity samples were not observed");

    api::CommandHeader_Request request;
    request.set_device_id("test-agv");
    api::CommandHeader_Feedback response;
    grpc::ServerContext context;
    const auto status = service_->cancelNavigation(
        &context, &request, &response);

    EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED);
    EXPECT_FALSE(response.success());
    EXPECT_NE(
        response.error_message().find("confirmed stopped state"),
        std::string::npos);
    EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_);
    EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_);

    const auto lease =
        control::ControlAuthorityManager::instance().tryAcquire(
            "test-agv",
            "normal-control-after-unconfirmed-stop",
            std::chrono::hours(1));
    EXPECT_FALSE(lease.acquired);

    auto& authority = control::ControlAuthorityManager::instance();
    const auto recovery = authority.preemptAcquire(
        "test-agv", "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(
        "test-agv", "normal-control-after-recovery", std::chrono::hours(1));
    EXPECT_TRUE(recovered.acquired) << recovered.detail;
    authority.release(recovered.token);
}

TEST_F(GrpcAgvServiceTest, StopMappingFailureReleasesOrdinaryControlLease)
{
    api::CommandHeader_Request request;
    request.set_device_id("test-agv");
    api::CommandHeader_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->stopMapping(
        &context, &request, &response);

    EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
    EXPECT_FALSE(response.success());
    auto& authority = control::ControlAuthorityManager::instance();
    const auto lease = authority.tryAcquire(
        "test-agv", "normal-control-after-mapping-failure",
        std::chrono::hours(1));
    EXPECT_TRUE(lease.acquired) << lease.detail;
    authority.release(lease.token);
}

TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams)
{
    api::AgvNavigateToStationCommand_Request request;
    request.mutable_header()->set_device_id("test-agv");
    request.set_station_id("AP1");
    auto* values = request.mutable_adapter_params()->mutable_values();
    (*values)["use_pgv"] = "true";
    (*values)["pgv_adjust_dist"] = "0.3";
    (*values)["pgv_adjust_cx"] = "-0.3";
    (*values)["pgv_adjust_cy"] = "0";
    api::AgvNavigateToStationCommand_Feedback response;
    grpc::ServerContext context;

    const auto status = service_->navigateToStation(
        &context,
        &request,
        &response);

    ASSERT_TRUE(status.ok()) << status.error_message();
    EXPECT_TRUE(response.header().success());
    EXPECT_EQ(agv_->station_id_, "AP1");
    EXPECT_EQ(
        agv_->station_adapter_params_.getString("use_pgv").value_or(""),
        "true");
    EXPECT_EQ(
        agv_->station_adapter_params_.getString("pgv_adjust_dist").value_or(""),
        "0.3");
    EXPECT_EQ(
        agv_->station_adapter_params_.getString("pgv_adjust_cx").value_or(""),
        "-0.3");
    EXPECT_EQ(
        agv_->station_adapter_params_.getString("pgv_adjust_cy").value_or(""),
        "0");
}

TEST_F(GrpcAgvServiceTest, NavigationErrorsMapToGrpcCodesAndPreserveDetails)
{
    struct ErrorCase {
        device::AgvErrorCode device_code;
        grpc::StatusCode grpc_code;
    };
    const ErrorCase cases[] = {
        {device::AgvErrorCode::InvalidArgument,
         grpc::StatusCode::INVALID_ARGUMENT},
        {device::AgvErrorCode::TaskCanceled,
         grpc::StatusCode::CANCELLED},
        {device::AgvErrorCode::Timeout,
         grpc::StatusCode::DEADLINE_EXCEEDED},
    };

    for (const auto& test_case : cases) {
        const std::string detail =
            "SEER Robokit navigation detail for code="
            + std::to_string(static_cast<int>(test_case.device_code));
        agv_->pose_result_ = device::AgvResult::failure(
            test_case.device_code,
            detail);
        api::AgvNavigateToPoseCommand_Request request;
        request.mutable_header()->set_device_id("test-agv");
        api::AgvNavigateToPoseCommand_Feedback response;
        grpc::ServerContext context;

        const auto status = service_->navigateToPose(
            &context,
            &request,
            &response);

        EXPECT_EQ(status.error_code(), test_case.grpc_code);
        EXPECT_EQ(status.error_message(), detail);
        EXPECT_FALSE(response.header().success());
        EXPECT_EQ(response.header().error_message(), detail);
    }
}

TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride)
{
    LegacyFollowPathAgv legacy;
    device::AbstractAGV* abstract = &legacy;
    const std::vector<device::AgvPathSegment> path = {
        {"station-1", "station-2"},
    };
    device::AgvMotionOptions options;

    const auto result = abstract->followPath(path, options);

    ASSERT_TRUE(result.ok()) << result.message;
    ASSERT_EQ(legacy.path_.size(), 1U);
    EXPECT_EQ(legacy.path_[0].source_station, "station-1");
    EXPECT_EQ(legacy.path_[0].target_station, "station-2");
}

TEST(AbstractAgvCompatibilityTest, SynchronousActionSupportDefaultsToFalse)
{
    LegacyFollowPathAgv legacy;

    EXPECT_FALSE(legacy.supportsSynchronousAction(
        device::AgvActionKind::NavigateToPose));
    EXPECT_FALSE(legacy.supportsSynchronousAction(
        device::AgvActionKind::NavigateToStation));
    EXPECT_FALSE(legacy.supportsSynchronousAction(
        device::AgvActionKind::FollowPath));
}

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