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

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

#include <memory>
#include <string>

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

#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "manager/device_manager/include/device_manager.h"

namespace cmvr::service {
namespace {

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

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

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

    device::AgvResult navigateToPose(
        const math::Pose2d& pose,
        const device::AgvMotionOptions& options,
        const device::AgvAdapterParams&) override
    {
        pose_ = pose;
        pose_options_ = options;
        return pose_result_;
    }

    device::AgvResult navigateToStation(
        const std::string& station_id,
        const device::AgvMotionOptions& options,
        const device::AgvAdapterParams&) override
    {
        station_id_ = station_id;
        station_options_ = options;
        return device::AgvResult::success();
    }

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

    math::Pose2d pose_;
    device::AgvMotionOptions pose_options_;
    device::AgvResult pose_result_{device::AgvResult::success()};
    std::string station_id_;
    device::AgvMotionOptions station_options_;
};

class GrpcAgvServiceTest : public ::testing::Test {
protected:
    void SetUp() override
    {
        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();
    }

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

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

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_);

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

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

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