feat:add grpc service runner

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

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_lambda{0.6};
-        double ibvs_mu{0.02};
+        double ibvs_mu{0.1};
-        double ibvs_qdot_max{0.6};
+        double ibvs_qdot_max{0.5};
-        std::array<double, 6> ibvs_vmax6{{0.4, 0.4, 0.4, 0.4, 0.4, 0.4}};
+        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()};
 

cmvr-es/applications/src/touch_screen_app.cpp

@@ -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_.setAlignCameraToVisp(options_.R_camera_to_visp);
-    ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
+    // 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;
 }
 

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* kDexhandId = "hand2";
-constexpr const char* kCameraId = "cam1";
+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 kTargetU = 1280 / 2.0;
-constexpr int kTargetV = 240;
+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);
 }

cmvr-es/main.cpp

@@ -7,72 +7,9 @@
 #include <libgen.h>
 #include <iostream>
 #include <glog/logging.h>
-#include "device_manager/include/device_manager.h"
+#include "service/grpc/include/server_runner.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>
-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"));
+    runner.start(config);
-    runServer(config);
+    runner.join();
     google::ShutdownGoogleLogging();
     return 0;
 }

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})

cmvr-es/service/grpc/include/server_runner.h

@@ -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};
+    };
+
+
+}

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<AbstractCamera>(dev_id);
         Rs2Intrinsics intrinsics = {0};

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 <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();
+    }
+}