update dexhand

include/service/grpc_dexhand_service.h

@@ -23,6 +23,9 @@ namespace cmvr::service {
         grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream) override;
     private:
         device::DeviceManager& dmgr_;
+        // 静态成员声明（关键！必须在类内声明）
+        static const std::unordered_map<std::string, int> kIdToIndex;  // 字符串id→索引
+        static const std::vector<std::string> kIndexToId;              // 索引→字符串id
     };
 
 }

include/service/grpc_humanoid_robot_service.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "device_manager/device_manager.h"
-#include "cmvr/api/humanoid_robot.grpc.pb.h"
+#include "cmvr/api/humanoid_robot_service.grpc.pb.h"
 
 namespace cmvr {
     namespace service {

protos/cmvr/api/dexhand_command.proto

@@ -4,146 +4,169 @@ import "cmvr/api/common.proto";
 
 package cmvr.api;
 
+// 自由度值（用于设置指令）
 message FreedomValue {
-    int32 id = 1;                 // 自由度id 0-6 分别对应六个自由度 小拇指 无名指 中指 食指 大拇指弯曲 大拇指旋转
-    float value = 2;              // 值 0-1百分比
+    string id = 1;                 // 自由度id，可选值：
+                                   // little_finger（小拇指）、
+                                   // ring_finger（无名指）、
+                                   // middle_finger（中指）、
+                                   // index_finger（食指）、
+                                   // thumb_bend（大拇指弯曲）、
+                                   // thumb_rotate（大拇指旋转）
+    float value = 2;               // 值 0-1百分比（对应实际物理量的比例）
 }
 
+// 自由度状态（用于获取状态指令的返回）
 message FreedomState {
-    int32 dof_id = 1;             // 自由度ID，唯一标识不同的关节或自由度
-    int32 angle = 2;              // 自由度角度
-    int32 speed = 3;              // 自由度速度
+    string dof_id = 1;             // 自由度id，与FreedomValue.id一致，可选值：
+                                   // little_finger（小拇指）、
+                                   // ring_finger（无名指）、
+                                   // middle_finger（中指）、
+                                   // index_finger（食指）、
+                                   // thumb_bend（大拇指弯曲）、
+                                   // thumb_rotate（大拇指旋转）
+    int32 angle = 2;               // 自由度当前角度
+    int32 speed = 3;               // 自由度当前速度
     int32 force = 4;               // 实际受力
-    int32 position = 5;           // 执行器位置
+    int32 position = 5;            // 执行器当前位置
     int32 current = 6;             // 执行器实际电流
     int32 temperature = 7;         // 执行器温度
-    int32 error = 8;              // 执行器故障信息
-    repeated string error_message = 9;  // 错误消息列表
+    int32 error = 8;               // 执行器故障码（0表示无故障，非0对应具体故障）
+    repeated string error_message = 9;  // 错误消息列表（详细描述故障原因）
 }
-// 传感器数据
+
 // 传感器数据
 message SensorData {
     // 手指类型（掌心作为特殊类型）
     enum FingerType {
         PINKY = 0;            // 小拇指
         RING = 1;             // 无名指
-        MIDDLE_FINGER = 2;   // 中指（避免与PartType.MIDDLE冲突）
+        MIDDLE_FINGER = 2;    // 中指（避免与PartType冲突）
         INDEX = 3;            // 食指
         THUMB = 4;            // 大拇指
         PALM = 5;             // 掌心
     }
 
-    // 部位类型（指中改为THUMB_MIDDLE以避免冲突）
+    // 部位类型（区分手指的不同部位）
     enum PartType {
-        TIP = 0;      // 指端
-        FINGER = 1;   // 指尖
-        PAD = 2;      // 指腹
-        THUMB_MIDDLE = 3;  // 大拇指指中（仅大拇指有）
-        PALM_PAD = 4;      // 掌心部位（对应PalmTactileData）
+        TIP = 0;              // 指端（指尖最前端）
+        FINGER = 1;           // 指节（手指中段）
+        PAD = 2;              // 指腹（手指接触物体的面）
+        THUMB_MIDDLE = 3;     // 大拇指指中（仅大拇指特有部位）
+        PALM_PAD = 4;         // 掌心部位（掌心接触区域）
     }
 
-    FingerType finger_type = 4;  // 手指类型（掌心使用PALM）
-    PartType part_type = 5;      // 部位类型
-    string sensor_name = 6;      // 传感器名称（如"小拇指指端"）
+    FingerType finger_type = 4;  // 所属手指（或掌心）
+    PartType part_type = 5;      // 所属部位
+    string sensor_name = 6;      // 传感器名称（如"little_finger_tip"，便于调试）
 
+    // 传感器单行数据
     message RowData {
-        repeated int32 values = 1 [packed = true];  // 一维数组，存放每行的浮点数值
+        repeated int32 values = 1 [packed = true];  // 单行传感器数据（原始采样值）
     }
-    repeated RowData data = 1;  // 二维数组，每行数据是 RowData 类型
-    int32 rows = 2;             // 行
-    int32 cols = 3;             // 列
+    repeated RowData data = 1;  // 二维传感器数据（按行列组织）
+    int32 rows = 2;             // 数据行数（传感器阵列行数）
+    int32 cols = 3;             // 数据列数（传感器阵列列数）
 }
 
-
+// 灵巧手整体状态
 message DexHandState {
-    bool is_initialized = 1;      // 是否初始化
-    repeated FreedomState hands = 2;  // 包含多个自由度状态
+    bool is_initialized = 1;      // 设备是否初始化完成（true：已就绪，false：未就绪）
+    repeated FreedomState hands = 2;  // 所有自由度的状态列表（与FreedomValue.id一一对应）
 }
 
+// 获取灵巧手状态指令
 message GetDexHandStateCommand {
     message Request {
-        CommandHeader.Request header = 1;
+        CommandHeader.Request header = 1;  // 通用请求头（包含设备ID等）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
-        DexHandState  state = 2;
+        CommandHeader.Feedback header = 1;  // 通用响应头（包含成功状态、时间戳等）
+        DexHandState state = 2;             // 灵巧手状态数据
     }
 }
 
+// 设置灵巧手位置指令
 message SetDexHandPositionsCommand {
     message Request {
-        CommandHeader.Request header = 1;
-        repeated FreedomValue values = 2;
+        CommandHeader.Request header = 1;  // 通用请求头
+        repeated FreedomValue values = 2;  // 待设置的位置参数（百分比）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
+        CommandHeader.Feedback header = 1;  // 通用响应头（反馈设置结果）
     }
 }
 
+// 设置灵巧手角度指令
 message SetDexHandAnglesCommand {
     message Request {
-        CommandHeader.Request header = 1;
-        repeated FreedomValue values = 2;
+        CommandHeader.Request header = 1;  // 通用请求头
+        repeated FreedomValue values = 2;  // 待设置的角度参数（百分比）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
+        CommandHeader.Feedback header = 1;  // 通用响应头
     }
 }
 
+// 设置灵巧手力值指令
 message SetDexHandForceCommand {
     message Request {
-        CommandHeader.Request header = 1;
-        repeated FreedomValue values = 2;
+        CommandHeader.Request header = 1;  // 通用请求头
+        repeated FreedomValue values = 2;  // 待设置的力值参数（百分比）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
+        CommandHeader.Feedback header = 1;  // 通用响应头
     }
 }
 
+// 设置灵巧手速度指令
 message SetDexHandSpeedCommand {
     message Request {
-        CommandHeader.Request header = 1;
-        repeated FreedomValue values = 2;
+        CommandHeader.Request header = 1;  // 通用请求头
+        repeated FreedomValue values = 2;  // 待设置的速度参数（百分比）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
+        CommandHeader.Feedback header = 1;  // 通用响应头
     }
 }
 
+// 设置灵巧手预设动作指令
 message SetDexHandPresetActCommand {
     message Request {
-        CommandHeader.Request header = 1;
-        int32 presetActId = 2;
+        CommandHeader.Request header = 1;  // 通用请求头
+        int32 presetActId = 2;             // 预设动作ID（如0：握拳，1：伸开等）
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
+        CommandHeader.Feedback header = 1;  // 通用响应头
     }
 }
 
+// 获取传感器数据指令（单次）
 message GetSensorDataCommand {
     message Request {
-        CommandHeader.Request header = 1;
+        CommandHeader.Request header = 1;  // 通用请求头
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
-        repeated SensorData sensor = 2;//所有传感器的数据
+        CommandHeader.Feedback header = 1;  // 通用响应头
+        repeated SensorData sensor = 2;     // 所有传感器的实时数据
     }
 }
 
+// 获取传感器数据指令（流模式）
 message GetSensorDataStreamCommand {
     message Request {
-        CommandHeader.Request header = 1;
+        CommandHeader.Request header = 1;  // 通用请求头
     }
 
     message Feedback {
-        CommandHeader.Feedback header = 1;
-        repeated SensorData sensor = 2;//所有传感器的数据
+        CommandHeader.Feedback header = 1;  // 通用响应头
+        repeated SensorData sensor = 2;     // 流式返回的传感器数据
     }
 }

protos/cmvr/api/humanoid_robot_command.proto

@@ -75,7 +75,7 @@ message SpeedJ{
   message Request{
     CommandHeader.Request header = 1;
     string joint_name = 2;
-    double vel = 3;
+    double vel = 3;  //速度 (rad/s)
     double acc = 4;
     RobotJointIndexDirection dir = 5;
   }

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -185,7 +185,7 @@ void HumanoidRobot<DOF>::init() {
     for (auto &task: tasks) task.get();
 
     rsm_.store(ROBOT_ESTOP);
-    LOG(INFO) << "All enabled motors initialized successfully.";
+    LOG(INFO) << "[HumanoidRobot](init):All enabled motors initialized successfully.";
 }
 
 template<int DOF>

src/service/grpc_dexhand_service.cpp

@@ -3,6 +3,8 @@
 //
 
 #include "service/grpc_dexhand_service.h"
+#include <unordered_map>
+#include <vector>
 using namespace std;
 using namespace cmvr::service;
 using namespace cmvr::device;
@@ -12,6 +14,26 @@ using namespace cmvr::device;
 #define DEXHAND_MAX_FORCE 3000
 #define DEXHAND_MAX_SPEED 1000
 
+// 1. 新增静态映射表（类内复用，避免重复定义）
+const unordered_map<string, int> gRPCDexHandServiceImpl::kIdToIndex = {
+    {"little_finger", 0},    // 小拇指 → 索引0
+    {"ring_finger", 1},      // 无名指 → 索引1
+    {"middle_finger", 2},    // 中指 → 索引2
+    {"index_finger", 3},     // 食指 → 索引3
+    {"thumb_bend", 4},       // 大拇指弯曲 → 索引4
+    {"thumb_rotate", 5}      // 大拇指旋转 → 索引5
+};
+
+// 2. 新增索引到字符串的映射（用于GetStatus返回string类型dof_id）
+const vector<string> gRPCDexHandServiceImpl::kIndexToId = {
+    "little_finger",
+    "ring_finger",
+    "middle_finger",
+    "index_finger",
+    "thumb_bend",
+    "thumb_rotate"
+};
+
 gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
 
 grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
@@ -23,9 +45,11 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
         DexHandState state;
         dev->getState(state);
         response->mutable_state()->set_is_initialized(state.is_initialized);
+
         for (int i = 0; i < 6; i++) {
             auto hand = response->mutable_state()->add_hands();
-            hand->set_dof_id(i);
+            // 关键修改：用索引映射表获取string类型dof_id，替代原int类型i
+            hand->set_dof_id(kIndexToId[i]);
             hand->set_angle(state.hands[i].angle);
             hand->set_current(state.hands[i].current);
             hand->set_force(state.hands[i].force);
@@ -37,6 +61,7 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
                 hand->add_error_message(errormessage);
             }
         }
+
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
@@ -48,6 +73,7 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
         return grpc::Status::OK;
     }
 }
+
 grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
     , const cmvr::api::SetDexHandPositionsCommand_Request* request
     , cmvr::api::SetDexHandPositionsCommand_Feedback* response){
@@ -57,12 +83,21 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
         auto freedoms = request->values();
 
-        //默认-1不修改状态
+        // 默认-1不修改状态（6个自由度）
         std::vector<int> finger_joint_targets(6, -1);
+
         for (auto& freedom : freedoms) {
-            //遍历拿到需要配置的自由度,传入的是百分比0-1
-            finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_POSITION;
+            const string& freedom_id = freedom.id();
+            // 查找字符串id对应的索引
+            auto it = kIdToIndex.find(freedom_id);
+            if (it == kIdToIndex.end()) {
+                throw invalid_argument("Invalid freedom id: " + freedom_id);
             }
+            // 计算目标位置（百分比转实际值）
+            int index = it->second;
+            finger_joint_targets[index] = freedom.value() * DEXHAND_MAX_POSITION;
+        }
+
         dev->setPositions(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -85,18 +120,19 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
         auto freedoms = request->values();
 
-        //默认-1不修改状态
+        // 默认-1不修改状态（6个自由度，与映射表数量一致）
         std::vector<int> finger_joint_targets(6, -1);
+
         for (auto& freedom : freedoms) {
-            //遍历拿到需要配置的自由度,传入的是百分比0-1
-            finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_ANGLE;
+            const std::string& freedom_id = freedom.id();
+            auto it = kIdToIndex.find(freedom_id);
+            if (it == kIdToIndex.end()) {
+                throw std::invalid_argument("Invalid freedom id: " + freedom_id);
             }
-        // std::cout << "steAngle: ";
-        // for (auto& angle : finger_joint_targets)
-        // {
-        //     std::cout << " " << angle;
-        // }
-        // std::cout << std::endl;
+            int index = it->second;
+            finger_joint_targets[index] = freedom.value() * DEXHAND_MAX_ANGLE;
+        }
+
         dev->setAngles(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -119,12 +155,21 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
         auto freedoms = request->values();
 
-        //默认-1不修改状态
+        // 默认-1不修改状态（6个自由度）
         std::vector<int> finger_joint_targets(6, -1);
+
         for (auto& freedom : freedoms) {
-            //遍历拿到需要配置的自由度,传入的是百分比0-1
-            finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_FORCE;
+            const string& freedom_id = freedom.id();
+            // 查找字符串id对应的索引
+            auto it = kIdToIndex.find(freedom_id);
+            if (it == kIdToIndex.end()) {
+                throw invalid_argument("Invalid freedom id: " + freedom_id);
             }
+            // 计算目标力值（百分比转实际值）
+            int index = it->second;
+            finger_joint_targets[index] = freedom.value() * DEXHAND_MAX_FORCE;
+        }
+
         dev->setForce(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -147,12 +192,21 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
         auto freedoms = request->values();
 
-        //默认-1不修改状态
+        // 默认-1不修改状态（6个自由度）
         std::vector<int> finger_joint_targets(6, -1);
+
         for (auto& freedom : freedoms) {
-            //遍历拿到需要配置的自由度,传入的是百分比0-1
-            finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_SPEED;
+            const string& freedom_id = freedom.id();
+            // 查找字符串id对应的索引
+            auto it = kIdToIndex.find(freedom_id);
+            if (it == kIdToIndex.end()) {
+                throw invalid_argument("Invalid freedom id: " + freedom_id);
             }
+            // 计算目标速度（百分比转实际值）
+            int index = it->second;
+            finger_joint_targets[index] = freedom.value() * DEXHAND_MAX_SPEED;
+        }
+
         dev->setVelocities(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@@ -195,22 +249,18 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
         string dev_id = request->header().device_id();
         LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id;
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
-        //从串口获取传感器数据并返回
         auto sensors = dev->getSensorData();
-        // 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData
+
         auto fillSensorData = [](const auto& tactileData,
                                  cmvr::api::SensorData::FingerType fingerType,
                                  cmvr::api::SensorData::PartType partType,
                                  cmvr::api::SensorData* sensorData) {
-            // 设置行列数
             sensorData->set_rows(tactileData.rows);
             sensorData->set_cols(tactileData.cols);
-            // 设置传感器类型
             sensorData->set_finger_type(fingerType);
             sensorData->set_part_type(partType);
-            sensorData->set_sensor_name(tactileData.name);  // 使用原结构体中的name字段
+            sensorData->set_sensor_name(tactileData.name);
 
-            // 填充数据
             for (const auto& row : tactileData.data) {
                 cmvr::api::SensorData_RowData* rowData = sensorData->add_data();
                 for (TactilePoint value : row) {
@@ -219,36 +269,24 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
             }
         };
 
-        // 1. 填充小拇指数据
         fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response->add_sensor());
         fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response->add_sensor());
         fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response->add_sensor());
-
-        // 2. 填充无名指数据
         fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response->add_sensor());
         fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response->add_sensor());
         fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response->add_sensor());
-
-        // 3. 填充中指数据（修改为MIDDLE_FINGER）
         fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response->add_sensor());
         fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response->add_sensor());
         fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response->add_sensor());
-
-        // 4. 填充食指数据
         fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response->add_sensor());
         fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response->add_sensor());
         fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response->add_sensor());
-
-        // 5. 填充大拇指数据（修改为THUMB_MIDDLE）
         fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response->add_sensor());
         fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response->add_sensor());
         fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response->add_sensor());
         fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response->add_sensor());
-
-        // 6. 填充掌心数据（使用PALM_PAD）
         fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response->add_sensor());
 
-
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
@@ -270,25 +308,22 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
         string dev_id = request.header().device_id();
         LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id;
         const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
+
         while (true)
         {
             api::GetSensorDataStreamCommand_Feedback response;
-            //从串口获取传感器数据并返回
             auto sensors = dev->getSensorData();
-            // 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData
+
             auto fillSensorData = [](const auto& tactileData,
                                      cmvr::api::SensorData::FingerType fingerType,
                                      cmvr::api::SensorData::PartType partType,
                                      cmvr::api::SensorData* sensorData) {
-                // 设置行列数
                 sensorData->set_rows(tactileData.rows);
                 sensorData->set_cols(tactileData.cols);
-                // 设置传感器类型
                 sensorData->set_finger_type(fingerType);
                 sensorData->set_part_type(partType);
-                sensorData->set_sensor_name(tactileData.name);  // 使用原结构体中的name字段
+                sensorData->set_sensor_name(tactileData.name);
 
-                // 填充数据
                 for (const auto& row : tactileData.data) {
                     cmvr::api::SensorData_RowData* rowData = sensorData->add_data();
                     for (TactilePoint value : row) {
@@ -297,33 +332,22 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
                 }
             };
 
-            // 1. 填充小拇指数据
             fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response.add_sensor());
             fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response.add_sensor());
             fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response.add_sensor());
-
-            // 2. 填充无名指数据
             fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response.add_sensor());
             fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response.add_sensor());
             fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response.add_sensor());
-
-            // 3. 填充中指数据（修改为MIDDLE_FINGER）
             fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response.add_sensor());
             fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response.add_sensor());
             fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response.add_sensor());
-
-            // 4. 填充食指数据
             fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response.add_sensor());
             fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response.add_sensor());
             fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response.add_sensor());
-
-            // 5. 填充大拇指数据（修改为THUMB_MIDDLE）
             fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response.add_sensor());
             fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response.add_sensor());
             fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response.add_sensor());
             fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response.add_sensor());
-
-            // 6. 填充掌心数据（使用PALM_PAD）
             fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response.add_sensor());
 
             if (!stream->Write(response)) {
@@ -338,6 +362,4 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
     catch (const std::exception& e) {
         return grpc::Status::OK;
     }
-
 }
-

src/service/grpc_humanoid_robot_service.cpp

@@ -5,6 +5,8 @@
 
 #include "service/grpc_humanoid_robot_service.h"
 #include <google/protobuf/util/time_util.h>
+
+#include "cmvr/api/humanoid_robot_command.pb.h"
 #include "robot/humanoid_robot/humanoid_robot.h"
 #include "cmvr/msgs/geometry.pb.h"
 
@@ -23,6 +25,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *contex
     grpc::Status ret = grpc::Status::OK;
 
     try {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (torqueOff): id=" << request->device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
         robot->torqueOff();
         response->set_success(true);
@@ -43,6 +46,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::torqueOn(grpc::ServerContext *context
     grpc::Status ret = grpc::Status::OK;
 
     try {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (torqueOn): id=" << request->device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->device_id());
         robot->torqueOn();
         response->set_success(true);
@@ -62,6 +66,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context,
     grpc::Status ret = grpc::Status::OK;
 
     try {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (moveJ): id=" << request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
         std::vector<JointPoint> cmds{};
         for (const auto& jc : request->cmds()) {
@@ -85,6 +90,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, c
 
     grpc::Status ret = grpc::Status::OK;
     try {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (moveL): id=" << request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
         auto ee_link = request->ee_link();
         msgs::Pose3d pose;
@@ -115,12 +121,16 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedJ(grpc::ServerContext* context,
     grpc::Status ret = grpc::Status::OK;
     try
     {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (speedJ): id=" << request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
         auto joint_name = request->joint_name();
         auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
         auto vel = request->vel();
         auto acc = request->acc();
         robot->speedJ(joint_name,dir,vel,acc);
+
+        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());
@@ -135,6 +145,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context,
     grpc::Status ret = grpc::Status::OK;
     try
     {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (speedL): id=" << request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
         auto ee_link = request->ee_link();
         auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
@@ -144,6 +155,9 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context,
 
         robot->setToolFrame(ee_link);
         robot->speedL(cart,dir,vel,acc);
+
+        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());
@@ -153,12 +167,12 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context,
     return ret;
 }
 
-grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context,
-    const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {
+grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {
 
     grpc::Status ret = grpc::Status::OK;
 
     try {
+        LOG(INFO) << "[gRPCHumanoidRobotServiceImpl] (getJointState): id=" << request->header().device_id();
         auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
 
         std::vector<cmvr::device::JointState> states{};