SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update robot controller

Browse Source
This commit is contained in:
linbo 2025-11-06 15:58:32 +08:00
parent 82ab7cd887
commit f5b765af3d
21 changed files with 114 additions and 567 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
config/dev_config.xml
View File

@ -69,21 +69,19 @@
<RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>
</DexHand>
<Robot>
<Robot id="" urdfPath="">
<ControllerManager>
<ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
<JointPositionCtrl defaultSpeed="" defaultAcc=""/>
<CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
</ComponentGroup>
<ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
<JointPositionCtrl/>
</ComponentGroup>
<ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
<JointPositionCtrl/>
</ComponentGroup>
</ControllerManager>
</Robot>
<Robot id="" urdfPath="">
<ControllerManager>
<ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
<JointPositionCtrl defaultSpeed="" defaultAcc=""/>
<CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
</ComponentGroup>
<ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
<JointPositionCtrl/>
</ComponentGroup>
<ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
<JointPositionCtrl/>
</ComponentGroup>
</ControllerManager>
</Robot>
<BioHead>

114
include/controller/touch_controller.h
View File

@ -1,114 +0,0 @@
//
// Created by lgv on 2025/8/24.
//
#pragma once
#include <utility>
#include "devices/abstract_camera.h"
#include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h"
#include "librealsense2/rs.h"
#include "librealsense2/h/rs_frame.h"
namespace cmvr {
namespace ctrl {
// 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
class PID {
public:
PID(double kp, double ki, double kd,
double i_max,
double output_max_pos, double output_max_neg,
double delta_max = 0.0) // 输出变化最大值，0 表示不限制
: kp_(kp), ki_(ki), kd_(kd),
i_max_(i_max),
output_max_pos_(output_max_pos),
output_max_neg_(output_max_neg),
delta_max_(delta_max),
prev_error_(0), integral_(0), prev_output_(0) {}
double compute(double target, double current, double dt, double deadband = 0.0) {
double error = target - current;
// 死区处理
if (fabs(error) <= deadband) {
error = 0.0;
}
// 积分累加限幅
integral_ += error * dt;
if (integral_ > i_max_) integral_ = i_max_;
if (integral_ < -i_max_) integral_ = -i_max_;
// 微分
double derivative = (error - prev_error_) / dt;
prev_error_ = error;
// PID 输出
double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
// 输出限幅
if (output > output_max_pos_) output = output_max_pos_;
if (output < -output_max_neg_) output = -output_max_neg_;
// 输出斜率限制
if (delta_max_ > 0.0) {
double delta = output - prev_output_;
if (delta > delta_max_) output = prev_output_ + delta_max_;
else if (delta < -delta_max_) output = prev_output_ - delta_max_;
}
prev_output_ = output;
return output;
}
private:
double kp_, ki_, kd_;
double prev_error_;
double integral_;
double i_max_; // 积分限幅
double output_max_pos_; // 向下按的最大输出
double output_max_neg_; // 向上抬的最大输出
double delta_max_; // 输出斜率限制
double prev_output_;
};
class TouchController {
public:
TouchController() {};
TouchController(std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand,std::shared_ptr<device::AbstractCamera> cam)
:robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)),
pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
~TouchController()=default;
bool isArrive(double max_force);
void touch(int u,int v ,double max_force);
void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
private:
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
std::shared_ptr<device::AbstractCamera> cam_{nullptr};
std::shared_ptr<PID> pid_{nullptr};
const double touch_threshold_ = 5.0; // 触控判定阈值
// 从压阻矩阵提取触控点与压力
bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
};
}
}

2
include/hardware/can/can_group.h
View File

@ -16,6 +16,8 @@ namespace cmvr::hardware
void init();
std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
uint8_t getNodeId(const std::string& joint_name);
private:
XmlNode cfg_;
std::string id_;

3
include/hardware/can/motor_protocol/motorprotocolmanager.h
View File

@ -21,7 +21,7 @@ namespace cmvr::hardware
public:
struct MotorInfo
{
std::string node_id; //电机ID
uint8_t node_id; //电机ID
std::string jointName; //关节名称
double limitQLb; //逆时针限位
double limitQUb; //顺时针限位
@ -32,6 +32,7 @@ namespace cmvr::hardware
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
~MotorProtocolManager() = default;
uint8_t getNodeId(const std::string& joint_name);
//根据协议类型获取协议实例对象
std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
private:

25
include/service/grpc_service/grpc_hlc_service.h
View File

@ -1,25 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#pragma once
#include "device_manager/device_manager.h"
#include "cmvr/api/hlc_service.grpc.pb.h"
namespace cmvr {
namespace service {
class gRPCHlcServiceImpl final : public api::HlcService::Service {
public:
gRPCHlcServiceImpl();
~gRPCHlcServiceImpl() = default;
grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
private:
device::DeviceManager& dmgr_;
};
}
}

58
src/controller/CMakeLists.txt
View File

@ -1,58 +0,0 @@
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
add_library(controller SHARED
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller.cpp
)
target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(controller PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
)
add_library(cmvr_es::ctrl::controller ALIAS controller)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)
link_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)
add_executable(touch_controller_test
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller_test.cpp
)
target_link_libraries(touch_controller_test
PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
cmvr_es::ctrl::controller
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

159
src/controller/touch_controller.cpp
View File

@ -1,159 +0,0 @@
//
// Created by lgv on 2025/8/24.
//
#include "controller/touch_controller.h"
using namespace cmvr::ctrl;
using namespace cmvr::msgs;
void TouchController::touch(std::shared_ptr<device::AbstractRobot> robot, const msgs::Pose3d pose, const msgs::Pose3d offset) {
msgs::Position target_position;
target_position.set_x(pose.position().x() - offset.position().x());
target_position.set_y(pose.position().y() - offset.position().y());
target_position.set_z(pose.position().z() - offset.position().z());
msgs::Euler target_euler;
target_euler.set_rx(pose.euler().rx() - offset.euler().rx());
target_euler.set_ry(pose.euler().ry() - offset.euler().ry());
target_euler.set_rz(pose.euler().rz() - offset.euler().rz());
msgs::Pose3d target_pose;
*target_pose.mutable_position() = target_position;
*target_pose.mutable_euler() = target_euler;
robot->moveJ("PELVIS_S","R_WRIST_R_S",target_pose);
}
void TouchController::touch(msgs::Pose3d pose, msgs::Pose3d offset, double max_force) {
auto hand_data = hand_->getSensorData();
double force = 0;
int x ,y;
extractTouch(hand_data.index.tip.data,force,x,y);
double dz = pid_->compute(max_force, force, 0.01,10);
std::cout << dz << std::endl;
// LOG(INFO) << "Force : " << force << " dz : " << dz;
// // 输出 3x3 数组
// LOG(INFO) << "Tip data (3x3):";
// for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) {
// std::stringstream ss;
// for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) {
// ss << hand_data.index.tip.data[i][j] << "\t";
// }
// LOG(INFO) << ss.str();
// }
}
bool TouchController::extractTouch(const std::vector<std::vector<uint16_t> > &matrix, double &force, int &x, int &y) {
int rows = matrix.size();
int cols = matrix[0].size();
double maxVal = 0;
int maxX = -1, maxY = -1;
double total = 0;
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
double val = matrix[i][j];
total += val;
if (val > maxVal) {
maxVal = val;
maxX = i;
maxY = j;
}
}
}
if (maxVal > touch_threshold_) {
force = total;
x = maxX;
y = maxY;
return true;
}
return false;
}
bool TouchController::isArrive(double max_force) {
const auto& hand_data = hand_->getSensorData();
double force = 0.0;
int x = 0, y = 0;
extractTouch(hand_data.index.tip.data, force, x, y);
LOG(INFO) << "Force : " << force ;
// 输出 3x3 数组
LOG(INFO) << "Tip data (3x3):";
for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) {
std::stringstream ss;
for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) {
ss << hand_data.index.tip.data[i][j] << "\t";
}
LOG(INFO) << ss.str();
}
return force > max_force;
}
void TouchController::touch(int u, int v, double max_force) {
LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force;
// 获取目标点和当前位姿
auto target_pose = cam_->get3DPointFromPixel(u, v);
auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP");
LOG(INFO) << "Target 3D Pose: " << target_pose;
// 定义关键点位
auto pre_touch_pose = cur_pose;
pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05);
pre_touch_pose.mutable_position()->set_y(target_pose[1]);
pre_touch_pose.mutable_position()->set_z(target_pose[2]);
auto touch_pose = pre_touch_pose;
touch_pose.mutable_position()->set_x(target_pose[0]);
const auto& retreat_pose = cur_pose;
// retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20);
// 1. 移动到预接触位置
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose);
// 2. 向前接触并开启压力监测
robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose);
// 3. 等待达到最大压力
constexpr int k_sleep_ms = 5;
constexpr int k_timeout_ms = 5000;
int elapsed_ms = 0;
while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) {
std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms));
elapsed_ms += k_sleep_ms;
}
if (elapsed_ms >= k_timeout_ms) {
LOG(WARNING) << "Timeout waiting for force feedback!";
} else {
LOG(INFO) << "Max force reached, retreating.";
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose);
}
}

71
src/controller/touch_controller_test.cpp
View File

@ -1,71 +0,0 @@
//
// Created by lgv on 2025/8/27.
//
#include "device_manager/device_manager.h"
#include "gtest/gtest.h"
#include "controller/touch_controller.h"
#include "cmvr/msgs/geometry.pb.h"
using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::ctrl;
TEST(TouchControllerTest,MyTest) {
//
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 1.49969573e-01);
pose.mutable_position()->set_y(-4.00100001e-01);
pose.mutable_position()->set_z(-1.00102800e-01);
pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(1.57);
robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
// robot->seJ("PELVIS_S","R_WRIST_R_S",pose);
// robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5);
cmvr::msgs::Pose3d delta_pose;
delta_pose.mutable_position()->set_x( 0.01);
delta_pose.mutable_position()->set_y(0);
delta_pose.mutable_position()->set_z(0);
delta_pose.mutable_euler()->set_rx(0);
delta_pose.mutable_euler()->set_ry(0);
delta_pose.mutable_euler()->set_rz(0);
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);
Pose3d offset;
TouchController controller(robot, hand,cam);
while (true) {
// controller.touch(pose,offset,500);
if (controller.isArrive(500)) {
delta_pose.mutable_position()->set_x( -0.01);
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05);
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}

7
src/devices/robot/controller/abstractcontroller.h
View File

@ -19,15 +19,16 @@ namespace cmvr::device
class AbstractController
{
public:
explicit AbstractController(const XmlNode& cfg);
virtual ~AbstractController();
explicit AbstractController(const XmlNode& cfg){}
virtual ~AbstractController(){}
[[nodiscard]] ControllerState getState() const {return state_;}
//此处的Json中应该包含目标电机信息，电机id，canGroupId等其他必要参数，用以确定是调用哪个can实例发送消息
// 还要包含操作内容，比如要执行的是直接控制每个电机位置，还是指定末端关节位置
/*
{“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]}
"params":{“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]},
"operate":{}
**/
virtual void call(const Json::Value& json) = 0;

4
src/devices/robot/controller/controller_manager.cpp
View File

@ -11,7 +11,7 @@ using namespace cmvr::device;
ControllerManager::ControllerManager(const XmlNode& cfg):state_(ControlManagerState_Idle)
{
create(cfg);
}
void ControllerManager::clearError()
@ -92,8 +92,6 @@ void ControllerManager::switchMode(ControlManagerState state)
break;
}
state_ = state;
}
ComponentGroup& ControllerManager::getComponentGroup(const std::string& id)

32
src/devices/robot/controller/jointpositioncontroller.cpp
View File

@ -21,11 +21,41 @@ void JointPositionController::call(const Json::Value& json)
//解析jason，执行算法
/*
"params":{
“canGroupId”:"leftArm",
"protocolType":"Ti5MotorProtocol",
"motors":[{"joint_name":""},{"joint_name":""}]
},
"operate":{}
**/
std::string canGroupId;
std::string protocolType;
if (json.isMember("params"))
{
Json::Value params = json["params"];
if (params.isMember("canGroupId"))
canGroupId = params["canGroupId"].asString();
if (params.isMember("protocolType"))
protocolType = params["protocolType"].asString();
if (params.isMember("motors"))
{
for (int i = 0; i < params["motors"].size(); i++)
{
std::string joint_name = params["motors"][i]["joint_name"].asString();
}
}
}
if (json.isMember("operate"))
{
}
//调用canmanager执行电机指令
HardWareManager::getInstance();
auto protocol = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorProtocol(protocolType);
//这里通过joint_name获取node_id?
}
void JointPositionController::interrupt()

1
src/devices/robot/humanoid_robot/CMakeLists.txt
View File

@ -11,6 +11,7 @@ target_link_libraries(humanoid_robot PRIVATE
cmvr_es::device::canbus
cmvr_es::device::ti5motor
protobuf::libprotobuf
cmvr_es::device::controller_manager
)

24
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -92,6 +92,11 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
[this] { update_state_(); });
rsm_.store(ROBOT_READY);
auto controllerManagerCfg = cfg.getChild("ControllerManager");
controller_manager_ = std::make_shared<ControllerManager>(controllerManagerCfg);
} catch (exception &e) {
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
throw runtime_error(e.what());
@ -306,6 +311,25 @@ void HumanoidRobot<DOF>::eStop() {
template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
try {
auto state = controller_manager_->getSate();
if (state == ControlManagerState_Teach
|| state == ControlManagerState_EStop
|| state == ControlManagerState_MajorFault)
{
throw runtime_error("Controller state error");
}
else if (state == ControlManagerState_Command)
{
//当前有命令正在执行，判断优先级？
//先获取当前正在执行的控制器
auto controller = controller_manager_->getActiveController();
Json::Value callJson;
Json::Value paramsJson;
Json::Value operateJson;
//拆分cmd，构建json内容
}
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {

5
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -31,6 +31,7 @@
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "../controller/controller_manager.h"
namespace cmvr::device{
@ -221,8 +222,8 @@ namespace cmvr::device{
std::shared_ptr<MotorManager> motor_manager_{nullptr};
private:
std::shared_ptr<ControllerManager> controller_manager_{nullptr};
};

9
src/hardware/can/can_group.cpp
View File

@ -51,4 +51,13 @@ void CanGroup::init()
}
std::shared_ptr<AbstractMotorProtocol> CanGroup::getMotorProtocol(const std::string &protocolType)
{
return motor_protocol_manager->getMotorProtocol(protocolType);
}
uint8_t CanGroup::getNodeId(const std::string& joint_name)
{
return motor_protocol_manager->getNodeId(joint_name);
}

24
src/hardware/can/motor_protocol/motorprotocolmanager.cpp
View File

@ -23,7 +23,11 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<de
if (node.getNodeName() == "Ti5MotorProtocol")
{
MotorInfo motor;
motor.node_id = node.getAttrString("id");
motor.node_id = node.getAttrDefault("id",0);
if (motor.node_id == 0)
{
throw std::runtime_error("MotorProtocolManager: Can't find motor id");
}
motor.jointName = node.getAttrString("joint_name");
motor.limitQd = node.getAttrDefault("limitQd",3.0f);
motor.limitQLb = node.getAttrDefault("limitQLb",3.14f);
@ -48,3 +52,21 @@ std::shared_ptr<AbstractMotorProtocol> MotorProtocolManager::getMotorProtocol(co
return motor_protocols_[protocolType];
return nullptr;
}
uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name)
{
uint8_t node_id = 0;
if (motors_.count("Ti5MotorProtocol"))
{
const auto motors = motors_["Ti5MotorProtocol"];
for (auto &motor: motors)
{
if (motor.jointName == joint_name)
{
node_id = motor.node_id;
break;
}
}
}
return node_id;
}

1
src/hardware_manager/hardware_manager.cpp
View File

@ -6,6 +6,7 @@
using namespace std;
using namespace cmvr::hardware;
std::shared_ptr<HardWareManager> HardWareManager::instance_ = nullptr;
std::once_flag HardWareManager::init_flag_;
HardWareManager::HardWareManager(const XmlNode& cfg) {
try {

3
src/main.cpp
View File

@ -18,7 +18,6 @@
#include "utils/base/logger.h"
#include "service/grpc_service/grpc_head_service.h"
#include "service/grpc_service/grpc_humanoid_robot_service.h"
#include "service/grpc_service/grpc_hlc_service.h"
#include "service/http_service/httpclient.h"
#include "json/json.h"
@ -55,7 +54,6 @@ void runServer(const XmlNode &cfg){
auto dexhand_service = gRPCDexHandServiceImpl();
auto biohand_service = gRPCMBioHeadServiceImpl();
auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl();
auto hlc_service = gRPCHlcServiceImpl();
grpc::ServerBuilder builder;
builder.AddListeningPort(address, grpc::InsecureServerCredentials());
@ -66,7 +64,6 @@ void runServer(const XmlNode &cfg){
builder.RegisterService(&dexhand_service);
builder.RegisterService(&biohand_service);
builder.RegisterService(&humanoid_robot_service);
builder.RegisterService(&hlc_service);
// 🔥 关键！启用反射
//grpc::reflection::InitProtoReflectionServerBuilderPlugin();

25
src/service/grpc_service/CMakeLists.txt
View File

@ -6,7 +6,6 @@ add_library(service
grpc_head_service.cpp
grpc_dexhand_service.cpp
grpc_humanoid_robot_service.cpp
grpc_hlc_service.cpp
)
target_include_directories(service PUBLIC ${PROJECT_SOURCE_DIR}/include)
@ -61,27 +60,3 @@ target_link_libraries(grpc_humanoid_robot_client_test
${OpenCV_LIBS}
)
add_executable(grpc_hlc_client_test
grpc_hlc_client_test.cpp
)
target_link_libraries(grpc_hlc_client_test
PRIVATE
cmvr_es::device::canbus
cmvr_es::device::ti5motor
cmvr_es::device::humanoid_robot
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

46
src/service/grpc_service/grpc_hlc_client_test.cpp
View File

@ -1,46 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#include "gtest/gtest.h"
#include <glog/logging.h>
#include <grpcpp/grpcpp.h>
#include "../../../include/service/grpc_service/grpc_hlc_service.h"
#include "google/protobuf/timestamp.pb.h"
#include <iostream>
#include <google/protobuf/util/time_util.h>
using namespace cmvr::api;
TEST(GrpcHlcClientTest, MyTest) {
// 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
auto stub = cmvr::api::HlcService::NewStub(channel);
grpc::ClientContext context;
cmvr::api::Touch_Request request;
cmvr::api::Touch_Response response;
request.mutable_header()->set_device_id("hc01");
*request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
request.set_u(12);
request.set_v(13);
request.set_max_force(1300);
// 调用
grpc::Status status = stub->touch(&context, request, &response);
if (status.ok()) {
LOG(INFO) << "Touch RPC succeeded." << std::endl;
LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl;
LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl;
LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl;
} else {
LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl;
}
}

40
src/service/grpc_service/grpc_hlc_service.cpp
View File

@ -1,40 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#include "service/grpc_service/grpc_hlc_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "controller/touch_controller.h"
using namespace cmvr::service;
using namespace cmvr::device;
using namespace cmvr::api;
using google::protobuf::util::TimeUtil;
gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){}
grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
grpc::Status ret = grpc::Status::OK;
try {
std::shared_ptr<AbstractRobot> robot = nullptr;
auto cam = dmgr_.getDevice<AbstractCamera>("cam4");
auto hand = dmgr_.getDevice<AbstractDexHand>("hand1");
ctrl::TouchController touch_controller(robot,hand,cam);;
// touch_controller.touch(request->u(),request->v(),request->max_force());
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;
}