From bf385669fc94371ec4c0307f8a3d753526e6d72e Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 13 Mar 2026 17:29:57 +0800 Subject: [PATCH] feat:add grpc service runner --- cmvr-es/applications/CMakeLists.txt | 2 + .../applications/include/touch_screen_app.h | 9 +- cmvr-es/applications/src/touch_screen_app.cpp | 13 +- .../src/touch_screen_app_test.cpp | 49 ++++- cmvr-es/main.cpp | 72 +------ cmvr-es/service/CMakeLists.txt | 1 + cmvr-es/service/grpc/include/server_runner.h | 56 ++++++ .../service/grpc/src/grpc_camera_service.cpp | 2 +- cmvr-es/service/grpc/src/server_runner.cpp | 186 ++++++++++++++++++ 9 files changed, 306 insertions(+), 84 deletions(-) create mode 100644 cmvr-es/service/grpc/include/server_runner.h create mode 100644 cmvr-es/service/grpc/src/server_runner.cpp diff --git a/cmvr-es/applications/CMakeLists.txt b/cmvr-es/applications/CMakeLists.txt index 3f31216f..7d4b7a1f 100644 --- a/cmvr-es/applications/CMakeLists.txt +++ b/cmvr-es/applications/CMakeLists.txt @@ -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 diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index 75769aa5..ad405585 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -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 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 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()}; diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index 3cfdd781..90b2a703 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -43,7 +43,7 @@ bool TouchScreenApp::init(const std::shared_ptr& 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; } diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp index 63d02fa9..9d2cb1eb 100644 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -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(kRobotId); - auto dexhand = dm.getDevice(kDexhandId); + // auto dexhand = dm.getDevice(kDexhandId); auto camera = dm.getDevice(kCameraId); + std::shared_ptr dexhand = nullptr; + + camera->start(); + + std::vector 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); } diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index 73f3ad91..ab0f76d3 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -7,72 +7,9 @@ #include #include #include -#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 -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(µphone_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(); - } -} - +using namespace cmvr::service; 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; } diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index ad63551a..27c95800 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -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}) diff --git a/cmvr-es/service/grpc/include/server_runner.h b/cmvr-es/service/grpc/include/server_runner.h new file mode 100644 index 00000000..be8eadf8 --- /dev/null +++ b/cmvr-es/service/grpc/include/server_runner.h @@ -0,0 +1,56 @@ +// +// Created by lgv on 3/13/26. +// +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#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 server_; + + std::string address_; + std::string last_error_; + + std::atomic running_{false}; + bool stop_requested_{false}; + bool started_{false}; + bool start_failed_{false}; + }; + + +} diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index e039b168..ce34dc9d 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -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(dev_id); Rs2Intrinsics intrinsics = {0}; diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp new file mode 100644 index 00000000..dc689aff --- /dev/null +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -0,0 +1,186 @@ +// +// Created by lgv on 3/13/26. +// + +#include "service/grpc/include/server_runner.h" + +#include +#include +#include +#include +#include +#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 +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 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 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 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(); + auto system_service = std::make_unique(); + auto speaker_service = std::make_unique(); + auto microphone_service = std::make_unique(); + auto dexhand_service = std::make_unique(); + auto biohand_service = std::make_unique(); + auto humanoid_robot_service = std::make_unique(); + auto hlc_service = std::make_unique(); + + 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 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 lk(mtx_); + need_stop = stop_requested_; + } + if (need_stop && server_) { + server_->Shutdown(); + } + + server_->Wait(); + + { + std::lock_guard lk(mtx_); + server_.reset(); + running_ = false; + } + } + catch (const std::exception& e) { + { + std::lock_guard lk(mtx_); + last_error_ = e.what(); + start_failed_ = true; + started_ = false; + running_ = false; + server_.reset(); + } + cv_.notify_all(); + LOG(ERROR) << e.what(); + } +}