84 lines
2.7 KiB
C++
84 lines
2.7 KiB
C++
|
//
|
||
|
|
// Created by lgv on 2025/8/15.
|
||
|
|
//
|
||
|
|
|
||
|
|
|
||
|
|
#include "service/grpc_humanoid_robot_service.h"
|
||
|
|
#include <google/protobuf/util/time_util.h>
|
||
|
|
#include "robot/humanoid_robot/humanoid_robot.h"
|
||
|
|
|
||
|
|
|
||
|
|
using namespace cmvr::service;
|
||
|
|
using namespace cmvr::device;
|
||
|
|
using namespace cmvr::api;
|
||
|
|
using google::protobuf::util::TimeUtil;
|
||
|
|
|
||
|
|
|
||
|
|
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()){}
|
||
|
|
|
||
|
|
|
||
|
|
grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context,
|
||
|
|
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) {
|
||
|
|
|
||
|
|
grpc::Status ret = grpc::Status::OK;
|
||
|
|
|
||
|
|
try {
|
||
|
|
auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
|
||
|
|
robot->torqueOff();
|
||
|
|
response->set_success(true);
|
||
|
|
response->set_error_message("");
|
||
|
|
}catch (const std::exception& e) {
|
||
|
|
response->set_success(false);
|
||
|
|
response->set_error_message(e.what());
|
||
|
|
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||
|
|
}
|
||
|
|
*response->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
grpc::Status gRPCHumanoidRobotServiceImpl::torqueOn(grpc::ServerContext *context,
|
||
|
|
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) {
|
||
|
|
|
||
|
|
grpc::Status ret = grpc::Status::OK;
|
||
|
|
|
||
|
|
try {
|
||
|
|
auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
|
||
|
|
robot->torqueOn();
|
||
|
|
response->set_success(true);
|
||
|
|
response->set_error_message("");
|
||
|
|
}catch (const std::exception& e) {
|
||
|
|
response->set_success(false);
|
||
|
|
response->set_error_message(e.what());
|
||
|
|
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||
|
|
}
|
||
|
|
*response->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context,
|
||
|
|
const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) {
|
||
|
|
grpc::Status ret = grpc::Status::OK;
|
||
|
|
|
||
|
|
try {
|
||
|
|
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
|
||
|
|
std::vector<JointPoint> cmds{};
|
||
|
|
for (const auto& jc : request->cmds()) {
|
||
|
|
cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel());
|
||
|
|
}
|
||
|
|
robot->moveJ(cmds,request->vel(),request->acc());
|
||
|
|
|
||
|
|
response->mutable_header()->set_success(true);
|
||
|
|
response->mutable_header()->set_error_message("");
|
||
|
|
}catch (const std::exception& e) {
|
||
|
|
response->mutable_header()->set_success(false);
|
||
|
|
response->mutable_header()->set_error_message(e.what());
|
||
|
|
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||
|
|
}
|
||
|
|
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
|