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

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

#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>

#include <google/protobuf/descriptor.h>
#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 {

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 device::Result::success(); }
    device::Result torqueOff() override { 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&) override
    {
        return device::Result::success();
    }
    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&,
        device::FrameType = device::FrameType::Base) override
    {
        return device::Result::success();
    }
    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
    {
        return device::Result::success();
    }

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

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

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

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

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