Merge remote-tracking branch 'origin/lgv_11_12' into lgv_11_12

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 open8 = 75;   // ch8 张嘴角 - 从75改为60
-    const double open9 = 105;  // ch9 张嘴角
+    const double open9 = 105;  // ch9 张嘴角 - 从105改为120
 
     // 眼睛舵机参数 (addr=64, channels=4~8)
-    const std::vector<double> eye_open_angles = {40, 120, 125, 50, 90};    // 睁开角度
+    const std::vector<double> eye_open_angles = {40, 120, 110, 60, 90};    // 睁开角度
     const std::vector<double> 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<int>(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<std::chrono::duration<double>>(now - start).count();
@@ -386,11 +391,21 @@ void BioHeadRobot::speakthread() {
             }
         }
 
-        // 正弦产生 0..1 区间因子（嘴角运动）
+        // 定期更新嘴角的随机扰动因子
-        double phase = 2.0 * M_PI * freq * t;
+        if (t - last_random_update > random_update_interval) {
-        double s = 0.5 * (1.0 + std::sin(phase)); // 0..1
+            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<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms) {
     std::vector<uint8_t> raw_data;
 
@@ -517,7 +533,7 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
     // 设备64角度（10通道）
     const std::vector<double> device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
     // 设备65角度（10通道）
-    const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
+    const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
 
     std::vector<uint8_t> raw_data_neutral;
 
@@ -547,38 +563,38 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
 //高兴
 void BioHeadRobot::expressionHappy() {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 70, 135, 120, 50, 90, 90};
-    const std::vector<double> device_65_angles = {100, 80, 125, 135, 100, 80, 70, 80, 85, 95};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 //惊讶
 void BioHeadRobot::expressionSurprised() {
     const std::vector<double> device_64_angles = {60, 90, 120, 90, 35, 160, 160, 25, 90, 90};
-    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 80, 100};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 //睡觉
 void BioHeadRobot::expressionTired() {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 75, 105, 105, 75, 90, 90};
-    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 
 //愤怒
 void BioHeadRobot::expressionAngry() {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 60, 120, 120, 60, 90, 90};
-    const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 //悲伤
 void BioHeadRobot::expressionSadness() {
     const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 115, 110, 70, 90, 90};
-    const std::vector<double> device_65_angles = {100, 80, 130, 55, 90, 55, 110, 85, 85, 95};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 //打哈欠
 void BioHeadRobot::expressionYawn() {
     const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
-    const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
+    const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
     sendExpression(device_64_angles, device_65_angles, 0);
 }
 

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<AbstractBiohead>(dev_id);
-        auto robot1 = dmgr_.getDevice<AbstractRobot>("hc01");
-        std::vector<JointPoint> 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<AbstractBiohead>(dev_id);
+
+
+            std::vector<JointPoint> 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;