From f47a93c21fcfb4404e123ee069b974697694c55e Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Thu, 4 Dec 2025 15:09:46 +0800 Subject: [PATCH] update --- config/cabin_robot.xml | 78 +++---- protos/cmvr/api/biohead_command.proto | 40 +++- protos/cmvr/api/biohead_service.proto | 11 + src/devices/biohead/abstract_biohead.h | 9 +- .../biohead_esp32/include/biohead_esp32.h | 3 + .../biohead_esp32/src/biohead_esp32.cpp | 217 ++++++++++++++++++ src/service/include/grpc_head_service.h | 5 + src/service/src/grpc_head_service.cpp | 74 ++++++ 8 files changed, 393 insertions(+), 44 deletions(-) diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 999536e0..08542302 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -32,47 +32,47 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - + diff --git a/protos/cmvr/api/biohead_command.proto b/protos/cmvr/api/biohead_command.proto index 04bcd472..cc91e3eb 100644 --- a/protos/cmvr/api/biohead_command.proto +++ b/protos/cmvr/api/biohead_command.proto @@ -196,7 +196,7 @@ message SpeakStop { -// 表情控制 - 高兴 +// 表情控制 - 微笑 message Happy { message Request { CommandHeader.Request header = 1; // 通用命令头 @@ -220,7 +220,7 @@ message Surprise { } - +//表情控制-疲惫 message ExpressionTired { message Request { CommandHeader.Request header = 1; @@ -231,4 +231,40 @@ message ExpressionTired { } } +//表情控制-愤怒 +message ExpressionAngry { + message Request { + CommandHeader.Request header = 1; + } + + message Feedback { + CommandHeader.Feedback header = 1; + } +} + + +//表情控制-悲伤 +message ExpressionSadness { + message Request { + CommandHeader.Request header = 1; + } + + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +//表情控制-打哈欠 +message ExpressionYawn { + message Request { + CommandHeader.Request header = 1; + } + + message Feedback { + CommandHeader.Feedback header = 1; + } +} + + + diff --git a/protos/cmvr/api/biohead_service.proto b/protos/cmvr/api/biohead_service.proto index 6206c5f5..f2dda323 100644 --- a/protos/cmvr/api/biohead_service.proto +++ b/protos/cmvr/api/biohead_service.proto @@ -31,6 +31,17 @@ service BioHeadService { rpc ExpressionTired(ExpressionTired.Request) returns (ExpressionTired.Feedback){}; + rpc ExpressionAngry(ExpressionAngry.Request) returns (ExpressionAngry.Feedback){}; + + rpc ExpressionSadness(ExpressionSadness.Request) returns (ExpressionSadness.Feedback){}; + + rpc ExpressionYawn(ExpressionYawn.Request) returns (ExpressionYawn.Feedback){}; + + + + + + } \ No newline at end of file diff --git a/src/devices/biohead/abstract_biohead.h b/src/devices/biohead/abstract_biohead.h index 4ebf5233..93e0eb20 100644 --- a/src/devices/biohead/abstract_biohead.h +++ b/src/devices/biohead/abstract_biohead.h @@ -68,9 +68,12 @@ namespace cmvr::device { virtual void speakstart(){}; virtual void speakstop(){}; - virtual void expressionHappy() {}; // 高兴 - virtual void expressionSurprised() {}; // 惊讶 - virtual void expressionTired() {}; // 困倦（疲惫） + virtual void expressionHappy() {}; + virtual void expressionSurprised() {}; + virtual void expressionTired() {}; + virtual void expressionAngry() {}; + virtual void expressionSadness() {}; + virtual void expressionYawn() {}; FacialExpressionState expression_state_; std::atomic emergency_stop_requested = false; diff --git a/src/devices/biohead/biohead_esp32/include/biohead_esp32.h b/src/devices/biohead/biohead_esp32/include/biohead_esp32.h index 6c96aa23..0e6937ae 100644 --- a/src/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/src/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -32,6 +32,9 @@ namespace cmvr::device { void expressionHappy()override; void expressionSurprised()override; void expressionTired()override; + void expressionAngry() override; + void expressionSadness()override; + void expressionYawn()override; private: diff --git a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index c5aa39d6..91d6ecf8 100644 --- a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -610,6 +610,36 @@ void BioHeadRobot::expressionHappy() { } serial_->sendRawServoData(raw_data); + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // 设备64角度（10通道） + const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + + std::vector raw_data_neutral; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_neutral[ch])); + raw_data_neutral.push_back(64); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_neutral[ch])); + raw_data_neutral.push_back(65); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data_neutral); } @@ -708,7 +738,194 @@ void BioHeadRobot::expressionTired() { } +void BioHeadRobot::expressionAngry() { + const int step_ms = 200; + // 设备64角度（10通道） + const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95}; + + std::vector raw_data; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_angles[ch])); + raw_data.push_back(64); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_angles[ch])); + raw_data.push_back(65); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data); + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // 设备64角度（10通道） + const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + + std::vector raw_data_neutral; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_neutral[ch])); + raw_data_neutral.push_back(64); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_neutral[ch])); + raw_data_neutral.push_back(65); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data_neutral); +} + +void BioHeadRobot::expressionSadness() { + const int step_ms = 200; + + // 设备64角度（10通道） + const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95}; + + std::vector raw_data; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_angles[ch])); + raw_data.push_back(64); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_angles[ch])); + raw_data.push_back(65); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data); + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // 设备64角度（10通道） + const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + + std::vector raw_data_neutral; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_neutral[ch])); + raw_data_neutral.push_back(64); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_neutral[ch])); + raw_data_neutral.push_back(65); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data_neutral); +} + +void BioHeadRobot::expressionYawn() { + const int step_ms = 200; + + // 设备64角度（10通道） + const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95}; + + std::vector raw_data; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_angles[ch])); + raw_data.push_back(64); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_angles[ch])); + raw_data.push_back(65); // 地址 + raw_data.push_back(ch); // 通道号 + raw_data.push_back(angle); // 角度 + raw_data.push_back(step_ms & 0xFF); // 低字节 + raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data); + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // 设备64角度（10通道） + const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + // 设备65角度（10通道） + const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; + + std::vector raw_data_neutral; + + // 下发设备64的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_64_neutral[ch])); + raw_data_neutral.push_back(64); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + // 下发设备65的10个通道（0~9） + for (int ch = 0; ch < 10; ++ch) { + uint8_t angle = static_cast(std::round(device_65_neutral[ch])); + raw_data_neutral.push_back(65); // 地址 + raw_data_neutral.push_back(ch); // 通道号 + raw_data_neutral.push_back(angle); // 角度 + raw_data_neutral.push_back(step_ms & 0xFF); // 低字节 + raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 + } + + serial_->sendRawServoData(raw_data_neutral); +} diff --git a/src/service/include/grpc_head_service.h b/src/service/include/grpc_head_service.h index 4b6552e9..e3875f9a 100644 --- a/src/service/include/grpc_head_service.h +++ b/src/service/include/grpc_head_service.h @@ -36,6 +36,11 @@ namespace cmvr::service grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override; + grpc::Status ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) override; + + grpc::Status ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) override; + + grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override; private: device::DeviceManager& dmgr_; }; diff --git a/src/service/src/grpc_head_service.cpp b/src/service/src/grpc_head_service.cpp index 42075e95..c5694f61 100644 --- a/src/service/src/grpc_head_service.cpp +++ b/src/service/src/grpc_head_service.cpp @@ -350,3 +350,77 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte return grpc::Status::OK; } } + + + +grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) +{ + try { + string dev_id = request->header().device_id(); + auto robot = dmgr_.getDevice(dev_id); + if (!robot) { + throw runtime_error("Biohead device not found"); + } + + robot->expressionAngry(); // 停止执行 + + response->mutable_header()->set_success(true); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + catch (const exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} + + + +grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) +{ + try { + string dev_id = request->header().device_id(); + auto robot = dmgr_.getDevice(dev_id); + if (!robot) { + throw runtime_error("Biohead device not found"); + } + + robot->expressionAngry(); // 停止执行 + + response->mutable_header()->set_success(true); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + catch (const exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} + + +grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) +{ + try { + string dev_id = request->header().device_id(); + auto robot = dmgr_.getDevice(dev_id); + if (!robot) { + throw runtime_error("Biohead device not found"); + } + + robot->expressionYawn(); // 停止执行 + + response->mutable_header()->set_success(true); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + catch (const exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +}