update robot controller

config/dev_config.xml

@@ -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="">
-                <Robot id="" urdfPath="">
+                <ControllerManager>
-                    <ControllerManager>
+                    <ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
-                        <ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
+                        <JointPositionCtrl defaultSpeed="" defaultAcc=""/>
-                            <JointPositionCtrl defaultSpeed="" defaultAcc=""/>
+                        <CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
-                            <CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
+                    </ComponentGroup>
-                        </ComponentGroup>
+                    <ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
-                        <ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
+                        <JointPositionCtrl/>
-                            <JointPositionCtrl/>
+                    </ComponentGroup>
-                        </ComponentGroup>
+                    <ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
-                        <ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
+                        <JointPositionCtrl/>
-                            <JointPositionCtrl/>
+                    </ComponentGroup>
-                        </ComponentGroup>
+                </ControllerManager>
-                    </ControllerManager>
-                </Robot>
             </Robot>
 
             <BioHead>

include/controller/touch_controller.h

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

include/hardware/can/can_group.h

@@ -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_;

include/hardware/can/motor_protocol/motorprotocolmanager.h

@@ -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:

include/service/grpc_service/grpc_hlc_service.h

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

src/controller/CMakeLists.txt

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

src/controller/touch_controller.cpp

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

src/controller/touch_controller_test.cpp

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

src/devices/robot/controller/abstractcontroller.h

@@ -19,15 +19,16 @@ namespace cmvr::device
     class AbstractController
     {
     public:
-        explicit AbstractController(const XmlNode& cfg);
+        explicit AbstractController(const XmlNode& cfg){}
-        virtual ~AbstractController();
+        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;
 

src/devices/robot/controller/controller_manager.cpp

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

src/devices/robot/controller/jointpositioncontroller.cpp

@@ -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()

src/devices/robot/humanoid_robot/CMakeLists.txt

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

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -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) {

src/devices/robot/humanoid_robot/humanoid_robot.h

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

src/hardware/can/can_group.cpp

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

src/hardware/can/motor_protocol/motorprotocolmanager.cpp

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

src/hardware_manager/hardware_manager.cpp

@@ -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 {

src/main.cpp

@@ -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();

src/service/grpc_service/CMakeLists.txt

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

src/service/grpc_service/grpc_hlc_client_test.cpp

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

src/service/grpc_service/grpc_hlc_service.cpp

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