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

feat:add grpc service runner

Browse Source
This commit is contained in:
lgv 2026-03-13 17:29:57 +08:00
parent 245ac5c3c8
commit bf385669fc
9 changed files with 306 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cmvr-es/applications/CMakeLists.txt
View File

@ -18,6 +18,8 @@ add_executable(touch_screen_app_test
target_link_libraries(touch_screen_app_test PRIVATE
cmvr_es::applications
cmvr_es::device_manager
cmvr_es::service
cmvr_es::monitor_manager
gtest
gtest_main
pthread

9
cmvr-es/applications/include/touch_screen_app.h
View File

@ -84,14 +84,15 @@ public:
double target_rz{0.0};
// IBVS 参数。
double ibvs_lambda{0.7};
double ibvs_mu{0.02};
double ibvs_qdot_max{0.6};
std::array<double, 6> ibvs_vmax6{{0.4, 0.4, 0.4, 0.4, 0.4, 0.4}};
double ibvs_lambda{0.6};
double ibvs_mu{0.1};
double ibvs_qdot_max{0.5};
std::array<double, 6> ibvs_vmax6{{0.04, 0.04, 0.04, 0.04, 0.04, 0.04}};
bool enable_joint_limit_avoidance{true};
double joint_limit_avoidance_gain{0.2};
double joint_limit_avoidance_margin_ratio{0.05};
double joint_limit_avoidance_max_push{0.25};
Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};

13
cmvr-es/applications/src/touch_screen_app.cpp
View File

@ -43,7 +43,7 @@ bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
camera_ = camera;
options_ = options;
if (!robot_ || !dexhand_ || !camera_) {
if (!robot_ || !camera_) {
initialized_ = false;
last_status_ = Status::INVALID_CONFIG;
return false;
@ -260,8 +260,15 @@ bool TouchScreenApp::applyOptions() {
options_.joint_limit_avoidance_gain,
options_.joint_limit_avoidance_margin_ratio,
options_.joint_limit_avoidance_max_push);
ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp);
ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
// ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp);
// ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
Eigen::Matrix3d RxPi;
RxPi << 1,0,0,
0,-1,0,
0,0,-1;
ibvs_.setAlignCameraToUrdf(RxPi);
ibvs_.setAlignCameraToVisp(RxPi);
return true;
}

49
cmvr-es/applications/src/touch_screen_app_test.cpp
View File

@ -6,25 +6,29 @@
#include "applications/include/touch_screen_app.h"
#include "device_manager/include/device_manager.h"
#include "service/grpc/include/server_runner.h"
namespace {
constexpr const char* kConfigPath =
"/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
"/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
// 这些 id 需要与现场配置一致；保持为示例调用中的写法。
constexpr const char* kRobotId = "hc01";
constexpr const char* kDexhandId = "dexhand1";
constexpr const char* kCameraId = "cam1";
constexpr const char* kDexhandId = "hand2";
constexpr const char* kCameraId = "right_hand_cam";
constexpr const char* kUrdfPath =
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf";
"/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf";
constexpr const char* kBaseLink = "PELVIS_S";
constexpr const char* kFlangeLink = "R_WRIST_R_S";
constexpr const char* kCameraLink = "R_CAM";
constexpr int kTargetU = 320;
constexpr int kTargetV = 240;
constexpr int kTargetU = 1280 / 2.0;
constexpr int kTargetV = 720 / 2.0;
void run_touch_once(int u, int v) {
const XmlNode config(kConfigPath);
cmvr::service::ServerRunner runner;
runner.start(config);
if (!config.hasChild("DeviceManager")) {
std::cerr << "DeviceManager node not found\n";
return;
@ -33,8 +37,27 @@ void run_touch_once(int u, int v) {
auto& dm = cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
auto robot = dm.getDevice<cmvr::device::AbstractRobot>(kRobotId);
auto dexhand = dm.getDevice<cmvr::device::AbstractDexHand>(kDexhandId);
// auto dexhand = dm.getDevice<cmvr::device::AbstractDexHand>(kDexhandId);
auto camera = dm.getDevice<cmvr::device::AbstractCamera>(kCameraId);
std::shared_ptr<cmvr::device::AbstractDexHand> dexhand = nullptr;
camera->start();
std::vector<cmvr::device::JointPoint> init_cmd{};
init_cmd = {
{"R_SHOULDER_P", -0.2423},
{"R_SHOULDER_R", 1.2929},
{"R_SHOULDER_Y", 1.61},
{"R_ELBOW_R", 1.58},
{"R_WRIST_P", -2.8792},
{"R_WRIST_Y", 0.1150},
{"R_WRIST_R", -0.08},
};
robot->moveJ(init_cmd,1.0,2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
cmvr::app::TouchScreenApp app;
@ -45,8 +68,9 @@ void run_touch_once(int u, int v) {
opt.flange_link = kFlangeLink;
opt.camera_link = kCameraLink;
opt.tag_size_m = 0.02;
opt.ibvs_vmax6 = {0.04, 0.04, 0.04, 0.04, 0.04, 0.04};
opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.12);
opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.30);
opt.pause_after_align_reached = true;
// 只测试视觉对齐阶段：禁止进入真实下压。
@ -63,6 +87,7 @@ void run_touch_once(int u, int v) {
opt.tactile_finger = cmvr::device::FingerType::INDEX;
opt.tactile_region = cmvr::app::TouchScreenApp::TactileRegion::FINGER;
opt.tactile_pressure_sum_threshold = 1e12;
opt.align_timeout_s = 100.0;
if (!app.init(robot, dexhand, camera, opt)) {
std::cerr << "TouchScreenApp init failed\n";
@ -103,10 +128,16 @@ void run_touch_once(int u, int v) {
<< cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
<< "\n";
}
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
}
} // namespace
TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) {
run_touch_once(kTargetU, kTargetV);
}

70
cmvr-es/main.cpp
View File

@ -7,72 +7,9 @@
#include <libgen.h>
#include <iostream>
#include <glog/logging.h>
#include "device_manager/include/device_manager.h"
#include "monitor_manager/include/monitor_manager.h"
#include "service/grpc/include/grpc_camera_service.h"
#include "service/grpc/include/grpc_system_service.h"
#include "service/grpc/include/grpc_speaker_service.h"
#include "service/grpc/include/grpc_microphone_service.h"
#include "service/grpc/include/grpc_dexhand_service.h"
#include "service/grpc/include/server_runner.h"
#include "utils/base/include/logger.h"
#include "service/grpc/include/grpc_head_service.h"
#include "service/grpc/include/grpc_humanoid_robot_service.h"
#include "service/grpc/include/grpc_hlc_service.h"
#include "json/json.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
void runServer(const XmlNode &cfg){
using namespace cmvr::device;
using namespace cmvr::service;
using namespace cmvr::monitor;
if (!cfg.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
return;
}
try {
// 🔥 关键！启用反射
grpc::reflection::InitProtoReflectionServerBuilderPlugin();
auto dmgr_cfg = cfg.getChild("DeviceManager");
DeviceManager::getInstance(dmgr_cfg);
auto mmgr_cfg = cfg.getChild("MonitorManager");
MonitorManager::getInstance(mmgr_cfg);
auto grpc_cfg = cfg.getChild("gRPCServer");
string port = grpc_cfg.getAttrDefault("port", "50051");
std::string address("0.0.0.0:" + port);
auto camera_service = gRPCCameraServiceImpl();
auto system_service = gRPCSystemServiceImpl();
auto speaker_service = gRPCSpeakerServiceImpl();
auto microphone_service = gRPCMicroPhoneServiceImpl();
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());
builder.RegisterService(&camera_service);
builder.RegisterService(&system_service);
builder.RegisterService(&speaker_service);
builder.RegisterService(&microphone_service);
builder.RegisterService(&dexhand_service);
builder.RegisterService(&biohand_service);
builder.RegisterService(&humanoid_robot_service);
builder.RegisterService(&hlc_service);
// 🔥 关键！启用反射
//grpc::reflection::InitProtoReflectionServerBuilderPlugin();
const std::unique_ptr server(builder.BuildAndStart());
std::cout << "Server listening on " << address << std::endl;
server->Wait();
}
catch (std:: exception &e) {
LOG(FATAL) << e.what();
}
}
int main(int argc, char* argv[]) {
std::string config_path;
if(argc == 1) {
@ -90,9 +27,10 @@ int main(int argc, char* argv[]) {
config_path = argv[1];
}
ServerRunner runner;
const XmlNode config(config_path);
initLogger(config.getChild("Logger"));
runServer(config);
runner.start(config);
runner.join();
google::ShutdownGoogleLogging();
return 0;
}

1
cmvr-es/service/CMakeLists.txt
View File

@ -8,6 +8,7 @@ add_library(service
grpc/src/grpc_dexhand_service.cpp
grpc/src/grpc_humanoid_robot_service.cpp
grpc/src/grpc_hlc_service.cpp
grpc/src/server_runner.cpp
)
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

56
cmvr-es/service/grpc/include/server_runner.h Normal file
View File

@ -0,0 +1,56 @@
//
// Created by lgv on 3/13/26.
//
#pragma once
#include <atomic>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <grpcpp/grpcpp.h>
#include "rapidxml/xml_parser.h"
namespace cmvr::service {
class ServerRunner {
public:
ServerRunner();
explicit ServerRunner(XmlNode cfg);
~ServerRunner();
ServerRunner(const ServerRunner&) = delete;
ServerRunner& operator=(const ServerRunner&) = delete;
ServerRunner(ServerRunner&&) = delete;
ServerRunner& operator=(ServerRunner&&) = delete;
void start(XmlNode cfg);
void stop();
void join();
bool isRunning() const;
std::string address() const;
private:
void threadMain();
private:
mutable std::mutex mtx_;
std::condition_variable cv_;
std::thread worker_;
XmlNode cfg_;
std::unique_ptr<grpc::Server> server_;
std::string address_;
std::string last_error_;
std::atomic<bool> running_{false};
bool stop_requested_{false};
bool started_{false};
bool start_failed_{false};
};
}

2
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -87,7 +87,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
{
try {
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
// LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
cv::Mat image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
Rs2Intrinsics intrinsics = {0};

186
cmvr-es/service/grpc/src/server_runner.cpp Normal file
View File

@ -0,0 +1,186 @@
//
// Created by lgv on 3/13/26.
//
#include "service/grpc/include/server_runner.h"
#include <unistd.h>
#include <limits.h>
#include <libgen.h>
#include <iostream>
#include <glog/logging.h>
#include "device_manager/include/device_manager.h"
#include "monitor_manager/include/monitor_manager.h"
#include "service/grpc/include/grpc_camera_service.h"
#include "service/grpc/include/grpc_system_service.h"
#include "service/grpc/include/grpc_speaker_service.h"
#include "service/grpc/include/grpc_microphone_service.h"
#include "service/grpc/include/grpc_dexhand_service.h"
#include "utils/base/include/logger.h"
#include "service/grpc/include/grpc_head_service.h"
#include "service/grpc/include/grpc_humanoid_robot_service.h"
#include "service/grpc/include/grpc_hlc_service.h"
#include "json/json.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
using namespace cmvr::service;
ServerRunner::ServerRunner() = default;
ServerRunner::ServerRunner(XmlNode cfg) {
start(std::move(cfg));
}
ServerRunner::~ServerRunner() {
stop();
join();
}
void ServerRunner::start(XmlNode cfg) {
std::unique_lock<std::mutex> lk(mtx_);
if (worker_.joinable()) {
throw std::runtime_error("ServerRunner already started");
}
cfg_ = std::move(cfg);
stop_requested_ = false;
started_ = false;
start_failed_ = false;
running_ = false;
last_error_.clear();
address_.clear();
server_.reset();
worker_ = std::thread(&ServerRunner::threadMain, this);
cv_.wait(lk, [this]() {
return started_ || start_failed_;
});
if (start_failed_) {
lk.unlock();
if (worker_.joinable()) {
worker_.join();
}
throw std::runtime_error(last_error_);
}
}
void ServerRunner::stop() {
std::unique_lock<std::mutex> lk(mtx_);
stop_requested_ = true;
if (server_) {
auto* s = server_.get();
lk.unlock();
s->Shutdown();
}
}
void ServerRunner::join() {
if (worker_.joinable()) {
worker_.join();
}
}
bool ServerRunner::isRunning() const {
return running_.load();
}
std::string ServerRunner::address() const {
std::lock_guard<std::mutex> lk(mtx_);
return address_;
}
void ServerRunner::threadMain() {
using namespace cmvr::device;
using namespace cmvr::service;
using namespace cmvr::monitor;
try {
if (!cfg_.hasChild("DeviceManager")) {
throw std::runtime_error("Device Manager node not found");
}
static std::once_flag reflection_once;
std::call_once(reflection_once, []() {
grpc::reflection::InitProtoReflectionServerBuilderPlugin();
});
auto dmgr_cfg = cfg_.getChild("DeviceManager");
DeviceManager::getInstance(dmgr_cfg);
if (cfg_.hasChild("MonitorManager")) {
auto mmgr_cfg = cfg_.getChild("MonitorManager");
MonitorManager::getInstance(mmgr_cfg);
}
auto grpc_cfg = cfg_.getChild("gRPCServer");
std::string port = grpc_cfg.getAttrDefault("port", "50051");
std::string local_address = "0.0.0.0:" + port;
auto camera_service = std::make_unique<gRPCCameraServiceImpl>();
auto system_service = std::make_unique<gRPCSystemServiceImpl>();
auto speaker_service = std::make_unique<gRPCSpeakerServiceImpl>();
auto microphone_service = std::make_unique<gRPCMicroPhoneServiceImpl>();
auto dexhand_service = std::make_unique<gRPCDexHandServiceImpl>();
auto biohand_service = std::make_unique<gRPCMBioHeadServiceImpl>();
auto humanoid_robot_service = std::make_unique<gRPCHumanoidRobotServiceImpl>();
auto hlc_service = std::make_unique<gRPCHlcServiceImpl>();
grpc::ServerBuilder builder;
builder.AddListeningPort(local_address, grpc::InsecureServerCredentials());
builder.RegisterService(camera_service.get());
builder.RegisterService(system_service.get());
builder.RegisterService(speaker_service.get());
builder.RegisterService(microphone_service.get());
builder.RegisterService(dexhand_service.get());
builder.RegisterService(biohand_service.get());
builder.RegisterService(humanoid_robot_service.get());
builder.RegisterService(hlc_service.get());
auto local_server = builder.BuildAndStart();
if (!local_server) {
throw std::runtime_error("Failed to build and start gRPC server");
}
{
std::lock_guard<std::mutex> lk(mtx_);
address_ = local_address;
server_ = std::move(local_server);
started_ = true;
running_ = true;
}
cv_.notify_all();
std::cout << "Server listening on " << local_address << std::endl;
bool need_stop = false;
{
std::lock_guard<std::mutex> lk(mtx_);
need_stop = stop_requested_;
}
if (need_stop && server_) {
server_->Shutdown();
}
server_->Wait();
{
std::lock_guard<std::mutex> lk(mtx_);
server_.reset();
running_ = false;
}
}
catch (const std::exception& e) {
{
std::lock_guard<std::mutex> lk(mtx_);
last_error_ = e.what();
start_failed_ = true;
started_ = false;
running_ = false;
server_.reset();
}
cv_.notify_all();
LOG(ERROR) << e.what();
}
}