diff --git a/include/service/grpc_dexhand_service.h b/include/service/grpc_dexhand_service.h index 87523de4..593d70f5 100644 --- a/include/service/grpc_dexhand_service.h +++ b/include/service/grpc_dexhand_service.h @@ -23,6 +23,9 @@ namespace cmvr::service { grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; private: device::DeviceManager& dmgr_; + // 静态成员声明(关键!必须在类内声明) + static const std::unordered_map kIdToIndex; // 字符串id→索引 + static const std::vector kIndexToId; // 索引→字符串id }; } diff --git a/include/service/grpc_humanoid_robot_service.h b/include/service/grpc_humanoid_robot_service.h index 498522a1..98ae76d3 100644 --- a/include/service/grpc_humanoid_robot_service.h +++ b/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 { diff --git a/protos/cmvr/api/dexhand_command.proto b/protos/cmvr/api/dexhand_command.proto index 9d65a7f8..ba3d5761 100644 --- a/protos/cmvr/api/dexhand_command.proto +++ b/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; // 自由度速度 - int32 force = 4; // 实际受力 - int32 position = 5; // 执行器位置 - int32 current = 6; // 执行器实际电流 - int32 temperature = 7; // 执行器温度 - int32 error = 8; // 执行器故障信息 - repeated string error_message = 9; // 错误消息列表 + 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 current = 6; // 执行器实际电流 + int32 temperature = 7; // 执行器温度 + int32 error = 8; // 执行器故障码(0表示无故障,非0对应具体故障) + repeated string error_message = 9; // 错误消息列表(详细描述故障原因) } -// 传感器数据 + // 传感器数据 message SensorData { // 手指类型(掌心作为特殊类型) enum FingerType { - PINKY = 0; // 小拇指 - RING = 1; // 无名指 - MIDDLE_FINGER = 2; // 中指(避免与PartType.MIDDLE冲突) - INDEX = 3; // 食指 - THUMB = 4; // 大拇指 - PALM = 5; // 掌心 + PINKY = 0; // 小拇指 + RING = 1; // 无名指 + 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; // 流式返回的传感器数据 } } \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto index a9c909a4..2317612d 100644 --- a/protos/cmvr/api/humanoid_robot_command.proto +++ b/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; } diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 22ce27e6..54a66a79 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -185,7 +185,7 @@ void HumanoidRobot::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 diff --git a/src/service/grpc_dexhand_service.cpp b/src/service/grpc_dexhand_service.cpp index aa752c45..b5305645 100644 --- a/src/service/grpc_dexhand_service.cpp +++ b/src/service/grpc_dexhand_service.cpp @@ -3,6 +3,8 @@ // #include "service/grpc_dexhand_service.h" +#include +#include 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 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 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(dev_id); auto freedoms = request->values(); - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); + // 默认-1不修改状态(6个自由度) + std::vector 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(dev_id); auto freedoms = request->values(); - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); + // 默认-1不修改状态(6个自由度,与映射表数量一致) + std::vector 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); + } + int index = it->second; + finger_joint_targets[index] = freedom.value() * DEXHAND_MAX_ANGLE; } - // std::cout << "steAngle: "; - // for (auto& angle : finger_joint_targets) - // { - // std::cout << " " << angle; - // } - // std::cout << std::endl; + 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(dev_id); auto freedoms = request->values(); - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); + // 默认-1不修改状态(6个自由度) + std::vector 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(dev_id); auto freedoms = request->values(); - //默认-1不修改状态 - std::vector finger_joint_targets(6,-1); + // 默认-1不修改状态(6个自由度) + std::vector 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(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(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; } - -} - +} \ No newline at end of file diff --git a/src/service/grpc_humanoid_robot_service.cpp b/src/service/grpc_humanoid_robot_service.cpp index b7797557..036609f4 100644 --- a/src/service/grpc_humanoid_robot_service.cpp +++ b/src/service/grpc_humanoid_robot_service.cpp @@ -5,6 +5,8 @@ #include "service/grpc_humanoid_robot_service.h" #include + +#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(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(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(request->header().device_id()); std::vector 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(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(request->header().device_id()); auto joint_name = request->joint_name(); auto dir = static_cast(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(request->header().device_id()); auto ee_link = request->ee_link(); auto dir = static_cast(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(request->header().device_id()); std::vector states{};