85 lines
2.3 KiB
C++
85 lines
2.3 KiB
C++
#include <grpcpp/grpcpp.h>
|
|
#include "common/base/logging/logger.h"
|
|
#include <gtest/gtest.h>
|
|
#include <google/protobuf/util/time_util.h>
|
|
|
|
#include "cmvr/api/arm_service.grpc.pb.h"
|
|
|
|
using google::protobuf::util::TimeUtil;
|
|
|
|
namespace {
|
|
|
|
std::unique_ptr<cmvr::api::ArmService::Stub> makeStub()
|
|
{
|
|
auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials());
|
|
return cmvr::api::ArmService::NewStub(channel);
|
|
}
|
|
|
|
void fillHeader(cmvr::api::CommandHeader_Request* header)
|
|
{
|
|
header->set_device_id("right_arm");
|
|
*header->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(GrpcArmClientTest, TorqueOn)
|
|
{
|
|
auto stub = makeStub();
|
|
cmvr::api::CommandHeader_Request request;
|
|
fillHeader(&request);
|
|
|
|
cmvr::api::CommandHeader_Feedback response;
|
|
grpc::ClientContext context;
|
|
const grpc::Status status = stub->torqueOn(&context, request, &response);
|
|
|
|
if (!status.ok()) {
|
|
CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
|
|
}
|
|
}
|
|
|
|
TEST(GrpcArmClientTest, MoveJ)
|
|
{
|
|
auto stub = makeStub();
|
|
cmvr::api::MoveJ_Request request;
|
|
fillHeader(request.mutable_header());
|
|
|
|
const std::vector<double> q{
|
|
0.00203898, 1.34062, 0.0, 0.322261, 0.0, -0.000210733, -0.0942364
|
|
};
|
|
for (double value : q) {
|
|
request.mutable_target()->add_position(value);
|
|
}
|
|
request.mutable_options()->set_velocity(0.8);
|
|
request.mutable_options()->set_acceleration(0.8);
|
|
|
|
cmvr::api::MoveJ_Response response;
|
|
grpc::ClientContext context;
|
|
const grpc::Status status = stub->moveJ(&context, request, &response);
|
|
|
|
if (!status.ok()) {
|
|
CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
|
|
}
|
|
}
|
|
|
|
TEST(GrpcArmClientTest, GetJointState)
|
|
{
|
|
auto stub = makeStub();
|
|
cmvr::api::JointRequest request;
|
|
fillHeader(request.mutable_header());
|
|
|
|
cmvr::api::JointResponse response;
|
|
grpc::ClientContext context;
|
|
const grpc::Status status = stub->getJointState(&context, request, &response);
|
|
|
|
if (status.ok()) {
|
|
const auto& state = response.state();
|
|
for (int i = 0; i < state.name_size() && i < state.position_size(); ++i) {
|
|
CMVR_LOG(INFO) << "Joint: " << state.name(i)
|
|
<< ", Position: " << state.position(i);
|
|
}
|
|
} else {
|
|
CMVR_LOG(ERROR) << "RPC failed: " << status.error_message();
|
|
}
|
|
}
|