From a6e7ea5bc169f6a4a7a55db904a0b6310aeadad7 Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Thu, 18 Dec 2025 18:35:51 +0800 Subject: [PATCH] update --- .../biohead_esp32/src/biohead_esp32.cpp | 50 ++++++++++++------- src/service/src/grpc_head_service.cpp | 8 +-- 2 files changed, 38 insertions(+), 20 deletions(-) diff --git a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index e17e59be..87c46c99 100644 --- a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -290,19 +290,20 @@ void BioHeadRobot::speakstop() { LOG(INFO) << "[BioHeadRobot] speak thread stopped."; } - void BioHeadRobot::speakthread() { LOG(INFO) << "[BioHeadRobot] speakthread running."; // 固定参数 const double freq = 3.0; // Hz const int step_ms = 40; // 下发周期（ms） + + // 修改嘴角参数，增大运动范围 const double closed_angle = 90.0; - const double open8 = 75; // ch8 张嘴角 - const double open9 = 105; // ch9 张嘴角 + const double open8 = 75; // ch8 张嘴角 - 从75改为60 + const double open9 = 105; // ch9 张嘴角 - 从105改为120 // 眼睛舵机参数 (addr=64, channels=4~8) - const std::vector eye_open_angles = {40, 120, 125, 50, 90}; // 睁开角度 + const std::vector eye_open_angles = {40, 120, 110, 60, 90}; // 睁开角度 const std::vector eye_close_angles = {90, 90, 90, 90, 90}; // 闭合角度 // 找到眼睛舵机索引 (addr=64, channels=4~8) @@ -311,7 +312,6 @@ void BioHeadRobot::speakthread() { if (channels_[i].addr == 64 && channels_[i].channel >= 4 && channels_[i].channel <= 8) { eye_indexes.push_back(static_cast(i)); } - } // 找到嘴角舵机索引（addr=65 ch=8/9） @@ -356,6 +356,11 @@ void BioHeadRobot::speakthread() { // 眼睛状态（true=睁开，false=闭合） bool eyes_open = true; + // 嘴角运动的随机扰动相关变量 + double last_random_update = 0.0; + double current_random_factor = 0.0; + const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动 + while (speak_running_.load()) { auto now = std::chrono::steady_clock::now(); double t = std::chrono::duration_cast>(now - start).count(); @@ -386,11 +391,21 @@ void BioHeadRobot::speakthread() { } } - // 正弦产生 0..1 区间因子（嘴角运动） - double phase = 2.0 * M_PI * freq * t; - double s = 0.5 * (1.0 + std::sin(phase)); // 0..1 + // 定期更新嘴角的随机扰动因子 + if (t - last_random_update > random_update_interval) { + current_random_factor = simple_rand(-0.2, 0.2); + last_random_update = t; + } - // 计算嘴角角度（直接使用度数，不用normalizeToAngle） + // 修改后的嘴角运动计算 + double phase = 2.0 * M_PI * freq * t; + + // 增加振幅并添加随机扰动 + double base_s = 0.5 * (1.0 + 1.3 * std::sin(phase)); + double s = base_s + current_random_factor; + s = std::clamp(s, 0.0, 1.0); + + // 计算嘴角角度 double target8 = closed_angle + (open8 - closed_angle) * s; double target9 = closed_angle + (open9 - closed_angle) * s; @@ -487,6 +502,7 @@ void BioHeadRobot::speakthread() { LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; } + void BioHeadRobot::sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms) { std::vector raw_data; @@ -517,7 +533,7 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c // 设备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}; + const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90}; std::vector raw_data_neutral; @@ -547,26 +563,26 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c //高兴 void BioHeadRobot::expressionHappy() { const std::vector device_64_angles = {90, 90, 90, 90, 70, 135, 120, 50, 90, 90}; - const std::vector device_65_angles = {100, 80, 125, 135, 100, 80, 70, 80, 85, 95}; + const std::vector device_65_angles = {100, 80, 125, 135, 100, 80, 70, 80, 80, 100}; sendExpression(device_64_angles, device_65_angles, 0); } //惊讶 void BioHeadRobot::expressionSurprised() { - const std::vector device_64_angles = {60, 90, 120, 90, 35, 160, 160, 25, 90, 90}; - const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 80, 100}; + const std::vector device_64_angles = {90, 100, 100, 80, 20, 140, 130, 50, 90, 90}; + const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 70, 110}; sendExpression(device_64_angles, device_65_angles, 0); } //睡觉 void BioHeadRobot::expressionTired() { const std::vector device_64_angles = {90, 90, 90, 90, 75, 105, 105, 75, 90, 90}; - const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97}; + const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 85, 95}; sendExpression(device_64_angles, device_65_angles, 0); } //愤怒 void BioHeadRobot::expressionAngry() { - const std::vector device_64_angles = {90, 90, 90, 90, 60, 120, 120, 60, 90, 90}; - const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95}; + const std::vector device_64_angles = {90, 90, 90, 90, 55, 125, 125, 55, 90, 90}; + const std::vector device_65_angles = {90, 90, 120, 130, 70, 90, 90, 130, 92, 88}; sendExpression(device_64_angles, device_65_angles, 0); } //悲伤 @@ -578,7 +594,7 @@ void BioHeadRobot::expressionSadness() { //打哈欠 void BioHeadRobot::expressionYawn() { const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; - const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95}; + const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90}; sendExpression(device_64_angles, device_65_angles, 0); } diff --git a/src/service/src/grpc_head_service.cpp b/src/service/src/grpc_head_service.cpp index 5c5e7638..e9130a9b 100644 --- a/src/service/src/grpc_head_service.cpp +++ b/src/service/src/grpc_head_service.cpp @@ -209,8 +209,6 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( try { string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); - auto robot1 = dmgr_.getDevice("hc01"); - std::vector cmds{}; if (!robot) { throw runtime_error("Biohead device not found"); @@ -219,7 +217,6 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( robot->eStop(); // 停止执行 robot->emergency_stop_requested = true; // ✅ 设置中断标志 - robot1->moveJ(cmds); @@ -240,15 +237,20 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c { { + try { string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); + + + std::vector cmds{}; if (!robot) { throw runtime_error("Biohead device not found"); } robot->speakstart(); // kaish开始 + response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK;