From 3748b59c3dd6923c7cac8ed78f569f72182888ec Mon Sep 17 00:00:00 2001 From: lgv Date: Thu, 13 Nov 2025 20:25:19 +0800 Subject: [PATCH] feat:head fun1 --- CMakeLists.txt | 2 +- config/cabin_robot.xml | 32 +- include/devices/abstract_biohead.h | 10 + include/devices/abstract_motor.h | 8 + include/service/grpc_head_service.h | 9 + protos/cmvr/api/biohead_command.proto | 57 ++ protos/cmvr/api/biohead_service.proto | 14 + .../biohead/biohead_esp32/biohead_esp32.cpp | 485 ++++++++++++++++++ .../biohead/biohead_esp32/biohead_esp32.h | 12 + src/devices/motor/motor_protocol_interface.h | 1 + .../canopen/protocol/ti5_motor_rpdo1.cpp | 14 +- .../canopen/protocol/ti5_motor_rpdo1.h | 2 +- .../canopen/ti5_motor_canopen_protocol.cpp | 10 +- .../canopen/ti5_motor_canopen_protocol.h | 1 + .../robot/humanoid_robot/humanoid_robot.cpp | 251 ++++----- src/main.cpp | 4 +- src/service/grpc_head_service.cpp | 123 +++++ 17 files changed, 888 insertions(+), 147 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c9594fc..1b45e96f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ protobuf_generate( PROTOC_OUT_DIR ${PROTO_BINARY_DIR} ) target_include_directories(proto-objects PUBLIC ${PROTO_BINARY_DIR} ${PROTO_IMPORT_DIR}) -target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++) +target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++ gRPC::grpc++_reflection) ############################################################ # include diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 0a620a29..67695934 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -40,7 +40,7 @@ bufferSize="50" verbose="false"> - + @@ -73,19 +73,27 @@ - - - - + - - - + + - - - - + + + + + + + diff --git a/include/devices/abstract_biohead.h b/include/devices/abstract_biohead.h index f6ee7833..86e2998d 100644 --- a/include/devices/abstract_biohead.h +++ b/include/devices/abstract_biohead.h @@ -65,10 +65,20 @@ namespace cmvr::device { virtual void eStop() {}; virtual void setExpressionPose(FacialExpressionState& expression_state, double vel=0, double acc=0) {} virtual void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) {} + virtual void speakstart(){}; + virtual void speakstop(){}; + + virtual void expressionHappy() {}; // 高兴 + virtual void expressionSurprised() {}; // 惊讶 + virtual void expressionTired() {}; // 困倦（疲惫） + FacialExpressionState expression_state_; std::atomic emergency_stop_requested = false; + + + }; } // namespace cmvr::device diff --git a/include/devices/abstract_motor.h b/include/devices/abstract_motor.h index d0b8d487..51e06615 100644 --- a/include/devices/abstract_motor.h +++ b/include/devices/abstract_motor.h @@ -111,6 +111,14 @@ namespace cmvr::device{ protocol_->setQ(node_id_, q); } + virtual void set(double q,double qd) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + throw std::runtime_error("Protocol not set for motor"); + } + protocol_->set(node_id_,q, qd); + } + virtual bool calibrateZeroQ() { std::scoped_lock lock(mtx_); if (!protocol_) { diff --git a/include/service/grpc_head_service.h b/include/service/grpc_head_service.h index ec36fa16..7933be8f 100644 --- a/include/service/grpc_head_service.h +++ b/include/service/grpc_head_service.h @@ -26,6 +26,15 @@ namespace cmvr::service grpc::Status EmergencyStop(grpc::ServerContext* context, const api::EmergencyStop_Request* request, api::EmergencyStop_Feedback* response) override; + grpc::Status SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) override; + + grpc::Status SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) override; + + grpc::Status Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) override; + + grpc::Status Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) override; + + grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override; private: device::DeviceManager& dmgr_; diff --git a/protos/cmvr/api/biohead_command.proto b/protos/cmvr/api/biohead_command.proto index 98527ce2..04bcd472 100644 --- a/protos/cmvr/api/biohead_command.proto +++ b/protos/cmvr/api/biohead_command.proto @@ -174,4 +174,61 @@ message EmergencyStop { } } +message SpeakStart { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + } +} + +message SpeakStop { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + } +} + + + +// 表情控制 - 高兴 +message Happy { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + } +} + + +// 表情控制 - 惊讶 +message Surprise { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + } +} + + + +message ExpressionTired { + 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 7ca4d831..6206c5f5 100644 --- a/protos/cmvr/api/biohead_service.proto +++ b/protos/cmvr/api/biohead_service.proto @@ -19,4 +19,18 @@ service BioHeadService { // 紧急停止 rpc EmergencyStop(EmergencyStop.Request) returns (EmergencyStop.Feedback){}; + + // 开始说话 + rpc SpeakStart(SpeakStart.Request) returns (SpeakStart.Feedback){}; + // 停止说话 + rpc SpeakStop(SpeakStop.Request) returns (SpeakStop.Feedback){}; + + rpc Happy(Happy.Request) returns (Happy.Feedback){}; + + rpc Surprise(Surprise.Request) returns (Surprise.Feedback){}; + + rpc ExpressionTired(ExpressionTired.Request) returns (ExpressionTired.Feedback){}; + + + } \ No newline at end of file diff --git a/src/devices/biohead/biohead_esp32/biohead_esp32.cpp b/src/devices/biohead/biohead_esp32/biohead_esp32.cpp index c043d1bf..e5f19542 100644 --- a/src/devices/biohead/biohead_esp32/biohead_esp32.cpp +++ b/src/devices/biohead/biohead_esp32/biohead_esp32.cpp @@ -227,6 +227,491 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou sendServoCommands(joints, duration); } + + +void BioHeadRobot::speakstart() { + + if (speak_running_.load()) { + LOG(INFO) << "[BioHeadRobot] speak thread already running."; + return; + } + + // 检查 channels 中是否有 65:8 和 65:9 + bool found8 = false, found9 = false; + for (size_t i = 0; i < channels_.size(); ++i) { + if (channels_[i].addr == 65 && channels_[i].channel == 8) found8 = true; + if (channels_[i].addr == 65 && channels_[i].channel == 9) found9 = true; + } + if (!found8 || !found9) { + LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; + return; + } + + // 启动线程 + speak_running_.store(true); + + // 清理旧线程（若有） + if (speak_thread_ && speak_thread_->joinable()) { + try { + speak_thread_->join(); + } catch (...) { + LOG(WARNING) << "[BioHeadRobot] exception joining previous speak thread."; + } + speak_thread_.reset(); + } + + speak_thread_ = std::make_shared(&BioHeadRobot::speakthread, this); + LOG(INFO) << "[BioHeadRobot] speak thread started."; +} + +void BioHeadRobot::speakstop() { + { + if (!speak_running_.load()) { + LOG(INFO) << "[BioHeadRobot] speak thread not running."; + return; + } + speak_running_.store(false); + } + // 唤醒线程（如果在 wait 中） + + // join 并清理线程对象 + if (speak_thread_) { + if (speak_thread_->joinable()) { + try { + speak_thread_->join(); + } catch (const std::exception &e) { + LOG(WARNING) << "[BioHeadRobot] exception when joining speak thread: " << e.what(); + } catch (...) { + LOG(WARNING) << "[BioHeadRobot] unknown exception when joining speak thread."; + } + } + speak_thread_.reset(); + } + 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 = 83.0; // ch8 张嘴角 +// const double open9 = 97.0; // ch9 张嘴角 +// +// // 找到 channels_ 中对应索引（addr=65 ch=8/9） +// int idx8 = -1, idx9 = -1; +// for (size_t i = 0; i < channels_.size(); ++i) { +// if (channels_[i].addr == 65 && channels_[i].channel == 8) idx8 = static_cast(i); +// if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast(i); +// } +// if (idx8 < 0 || idx9 < 0) { +// LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread."; +// speak_running_.store(false); +// return; +// } +// +// // 以当前角度为基准 +// std::vector base = current_joints_; +// if (base.size() != channels_.size()) base.resize(channels_.size(), 90.0); +// +// auto start = std::chrono::steady_clock::now(); +// +// while (speak_running_.load()) { +// auto now = std::chrono::steady_clock::now(); +// double t = std::chrono::duration_cast>(now - start).count(); +// +// // 正弦产生 0..1 区间因子 +// double phase = 2.0 * M_PI * freq * t; +// double s = 0.5 * (1.0 + std::sin(phase)); // 0..1 +// +// // 计算角度（直接使用度数，不用normalizeToAngle） +// double target8 = closed_angle + (open8 - closed_angle) * s; +// double target9 = closed_angle + (open9 - closed_angle) * s; +// +// // clamp 到限位 +// target8 = std::clamp(target8, min_angles_[idx8], max_angles_[idx8]); +// target9 = std::clamp(target9, min_angles_[idx9], max_angles_[idx9]); +// +// // 构建目标角数组（只修改 idx8/idx9） +// std::vector tgt = base; +// tgt[idx8] = target8; +// tgt[idx9] = target9; +// +// // ---- 构造 ESP32 协议帧 ---- +// std::vector raw_data; +// for (int idx : {idx8, idx9}) { +// uint16_t angle_raw = static_cast(std::round(tgt[idx])); // 直接角度 +// raw_data.push_back(channels_[idx].addr); // 地址 +// raw_data.push_back(channels_[idx].channel); // 通道 +// raw_data.push_back(angle_raw); // 角度（1字节即可） +// raw_data.push_back(step_ms & 0xFF); // 持续时间低字节 +// raw_data.push_back((step_ms >> 8) & 0xFF); // 持续时间高字节 +// } +// +// // 打印发送数据 +// std::string json_output = "{ \"raw_data\": ["; +// for (size_t i = 0; i < raw_data.size(); ++i) { +// json_output += std::to_string(raw_data[i]); +// if (i < raw_data.size() - 1) json_output += ", "; +// } +// json_output += "] }"; +// std::cout << "Sending data at step: " << json_output << std::endl; +// +// // 下发 +// serial_->sendRawServoData(raw_data); +// +// // 控制循环频率 +// std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); +// } +// +// // 退出前恢复基准角 +// std::vector restore_data; +// for (int idx : {idx8, idx9}) { +// uint16_t angle_raw = static_cast(std::round(base[idx])); +// restore_data.push_back(channels_[idx].addr); +// restore_data.push_back(channels_[idx].channel); +// restore_data.push_back(angle_raw); +// restore_data.push_back(200 & 0xFF); +// restore_data.push_back((200 >> 8) & 0xFF); +// } +// serial_->sendRawServoData(restore_data); +// LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; +// } + +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 = 80.0; // ch8 张嘴角 + const double open9 = 100.0; // ch9 张嘴角 + + // 眼睛舵机参数 (addr=64, channels=4~8) + const std::vector eye_open_angles = {40, 120, 125, 50, 75}; // 睁开角度 + const std::vector eye_close_angles = {90, 90, 90, 90, 75}; // 闭合角度 + + // 找到眼睛舵机索引 (addr=64, channels=4~8) + std::vector eye_indexes; + for (size_t i = 0; i < channels_.size(); ++i) { + 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） + int idx8 = -1, idx9 = -1; + for (size_t i = 0; i < channels_.size(); ++i) { + if (channels_[i].addr == 65 && channels_[i].channel == 8) idx8 = static_cast(i); + if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast(i); + } + + if (idx8 < 0 || idx9 < 0) { + LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread."; + speak_running_.store(false); + return; + } + + // 以当前角度为基准 + std::vector base = current_joints_; + if (base.size() != channels_.size()) { + base.resize(channels_.size(), 90.0); + } + + // 简单的伪随机数生成（避免依赖C++11随机数库） + unsigned int seed = static_cast(std::chrono::system_clock::now().time_since_epoch().count()); + + // 计时器 + auto start = std::chrono::steady_clock::now(); + auto last_blink_time = std::chrono::steady_clock::now(); + + // 使用简单的线性同余生成器 + auto simple_rand = [&seed](double min_val, double max_val) -> double { + seed = (seed * 1103515245 + 12345) & 0x7FFFFFFF; + return min_val + (max_val - min_val) * (seed / 2147483647.0); + }; + + double next_blink_interval = simple_rand(2.0, 8.0); // 2-8秒随机眨眼间隔 + + // 状态变量 + bool is_blinking = false; + auto blink_start_time = std::chrono::steady_clock::now(); + double blink_duration = 0.0; + + // 眼睛状态（true=睁开，false=闭合） + bool eyes_open = true; + + while (speak_running_.load()) { + auto now = std::chrono::steady_clock::now(); + double t = std::chrono::duration_cast>(now - start).count(); + + // 检查是否需要开始眨眼 + if (!is_blinking && eyes_open) { + double time_since_last_blink = std::chrono::duration_cast>(now - last_blink_time).count(); + if (time_since_last_blink >= next_blink_interval) { + is_blinking = true; + blink_start_time = now; + blink_duration = simple_rand(0.1, 0.3); // 眨眼持续时间0.1-0.3秒 + eyes_open = false; + LOG(INFO) << "[BioHeadRobot] Starting blink, duration: " << blink_duration << "s"; + } + } + + // 处理眨眼状态 + if (is_blinking) { + double blink_elapsed = std::chrono::duration_cast>(now - blink_start_time).count(); + + if (blink_elapsed >= blink_duration) { + // 眨眼结束 + is_blinking = false; + eyes_open = true; + last_blink_time = now; + next_blink_interval = simple_rand(2.0, 8.0); + LOG(INFO) << "[BioHeadRobot] Blink completed, next blink in: " << next_blink_interval << "s"; + } + } + + // 正弦产生 0..1 区间因子（嘴角运动） + double phase = 2.0 * M_PI * freq * t; + double s = 0.5 * (1.0 + std::sin(phase)); // 0..1 + + // 计算嘴角角度（直接使用度数，不用normalizeToAngle） + double target8 = closed_angle + (open8 - closed_angle) * s; + double target9 = closed_angle + (open9 - closed_angle) * s; + + // clamp 到限位 + target8 = std::clamp(target8, min_angles_[idx8], max_angles_[idx8]); + target9 = std::clamp(target9, min_angles_[idx9], max_angles_[idx9]); + + // 构建目标角数组 + std::vector tgt = base; + + // 设置嘴角 + tgt[idx8] = target8; + tgt[idx9] = target9; + + // 设置眼睛状态 + if (eye_indexes.size() == 5) { + const std::vector& eye_angles = eyes_open ? eye_open_angles : eye_close_angles; + for (size_t i = 0; i < eye_indexes.size(); ++i) { + if (eye_indexes[i] < tgt.size()) { + tgt[eye_indexes[i]] = eye_angles[i]; + } + } + } + + // ---- 构造 ESP32 协议帧 ---- + std::vector raw_data; + + // 添加嘴角控制 + for (int idx : {idx8, idx9}) { + uint8_t angle_raw = static_cast(std::round(tgt[idx])); + raw_data.push_back(channels_[idx].addr); + raw_data.push_back(channels_[idx].channel); + raw_data.push_back(angle_raw); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + + // 添加眼睛控制 + if (eye_indexes.size() == 5) { + for (int idx : eye_indexes) { + uint8_t angle_raw = static_cast(std::round(tgt[idx])); + raw_data.push_back(channels_[idx].addr); + raw_data.push_back(channels_[idx].channel); + raw_data.push_back(angle_raw); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + } + + // 打印发送数据（调试用） + if (static_cast(t * 1000) % 100 == 0) { // 每100个周期打印一次，避免过于频繁 + std::string json_output = "{ \"raw_data\": ["; + for (size_t i = 0; i < raw_data.size(); ++i) { + json_output += std::to_string(raw_data[i]); + if (i < raw_data.size() - 1) json_output += ", "; + } + json_output += "] }"; + std::cout << "Sending data: " << json_output << std::endl; + } + + // 下发 + serial_->sendRawServoData(raw_data); + + // 控制循环频率 + std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); + } + + // 退出前恢复基准角 + std::vector restore_data; + + // 恢复嘴角 + for (int idx : {idx8, idx9}) { + uint8_t angle_raw = static_cast(std::round(base[idx])); + restore_data.push_back(channels_[idx].addr); + restore_data.push_back(channels_[idx].channel); + restore_data.push_back(angle_raw); + restore_data.push_back(200 & 0xFF); + restore_data.push_back((200 >> 8) & 0xFF); + } + + // 恢复眼睛（确保睁开） + if (eye_indexes.size() == 5) { + for (size_t i = 0; i < eye_indexes.size(); ++i) { + uint8_t angle_raw = static_cast(std::round(eye_open_angles[i])); + restore_data.push_back(channels_[eye_indexes[i]].addr); + restore_data.push_back(channels_[eye_indexes[i]].channel); + restore_data.push_back(angle_raw); + restore_data.push_back(200 & 0xFF); + restore_data.push_back((200 >> 8) & 0xFF); + } + } + + serial_->sendRawServoData(restore_data); + LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; +} + + + + +void BioHeadRobot::expressionHappy() { + const int step_ms = 200; + + // 设备64角度（10通道） + const std::vector device_64_angles = {90, 90, 90, 90, 70, 110, 110, 70, 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); +} + + +void BioHeadRobot::expressionSurprised() { + const int step_ms = 200; + + const std::vector device_64_angles = {60, 170, 120, 90, 60, 120, 120, 60, 90, 90}; + const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97}; + + std::vector raw_data; + + for (size_t i = 0; i < 10; ++i) { + int idx = -1; + for (size_t j = 0; j < channels_.size(); ++j) { + if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) { + idx = j; + break; + } + } + if (idx >= 0) { + uint8_t angle = static_cast(std::round(device_64_angles[i])); + raw_data.push_back(64); + raw_data.push_back(4 + i); + raw_data.push_back(angle); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + } + + for (size_t i = 0; i < 10; ++i) { + int idx = -1; + for (size_t j = 0; j < channels_.size(); ++j) { + if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) { + idx = j; + break; + } + } + if (idx >= 0) { + uint8_t angle = static_cast(std::round(device_65_angles[i])); + raw_data.push_back(65); + raw_data.push_back(8 + i); + raw_data.push_back(angle); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + } + + serial_->sendRawServoData(raw_data); +} + +void BioHeadRobot::expressionTired() { + const int step_ms = 200; + + 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, 90, 90}; + + std::vector raw_data; + + for (size_t i = 0; i < 10; ++i) { + int idx = -1; + for (size_t j = 0; j < channels_.size(); ++j) { + if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) { + idx = j; + break; + } + } + if (idx >= 0) { + uint8_t angle = static_cast(std::round(device_64_angles[i])); + raw_data.push_back(64); + raw_data.push_back(4 + i); + raw_data.push_back(angle); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + } + + for (size_t i = 0; i < 10; ++i) { + int idx = -1; + for (size_t j = 0; j < channels_.size(); ++j) { + if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) { + idx = j; + break; + } + } + if (idx >= 0) { + uint8_t angle = static_cast(std::round(device_65_angles[i])); + raw_data.push_back(65); + raw_data.push_back(8 + i); + raw_data.push_back(angle); + raw_data.push_back(step_ms & 0xFF); + raw_data.push_back((step_ms >> 8) & 0xFF); + } + } + + serial_->sendRawServoData(raw_data); +} + + + + + + void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_t duration_ms) { std::vector addrs, chs; std::vector raws; diff --git a/src/devices/biohead/biohead_esp32/biohead_esp32.h b/src/devices/biohead/biohead_esp32/biohead_esp32.h index 892e1f0a..e864cb29 100644 --- a/src/devices/biohead/biohead_esp32/biohead_esp32.h +++ b/src/devices/biohead/biohead_esp32/biohead_esp32.h @@ -25,6 +25,13 @@ namespace cmvr::device { void eStop() override; void setExpressionPose(FacialExpressionState& expression_state, double vel = 0.5, double acc = 0.1) override; void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override; + void speakstart() override; + void speakstop() override; + void speakthread(); + + void expressionHappy()override; + void expressionSurprised()override; + void expressionTired()override; private: @@ -34,6 +41,7 @@ namespace cmvr::device { uint16_t angleToRaw(double angle); double normalizeToAngle(double normalized, size_t index); + std::shared_ptr serial_; std::string port_name_; @@ -48,6 +56,10 @@ namespace cmvr::device { double angle_deadband_{36}; std::vector last_joints_; // 上次下发角度缓存 + std::shared_ptr speak_thread_; + std::atomic speak_running_{false}; + + }; diff --git a/src/devices/motor/motor_protocol_interface.h b/src/devices/motor/motor_protocol_interface.h index b24dc466..9d949f7c 100644 --- a/src/devices/motor/motor_protocol_interface.h +++ b/src/devices/motor/motor_protocol_interface.h @@ -27,6 +27,7 @@ namespace cmvr { virtual bool initNode(uint8_t node_id) = 0; virtual void setQ(uint8_t node_id, double angle_rad) = 0; + virtual void set(uint8_t node_id, double angle_rad,double vel) = 0; virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; diff --git a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp index 42ba1b9f..88c655a2 100644 --- a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp +++ b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp @@ -15,12 +15,12 @@ void Ti5MotorRPDO1::UpdateData(uint8_t *data) { data[1] = target_pos_ >> 8 & 0xFF; data[2] = target_pos_ >> 16 & 0xFF; data[3] = target_pos_ >> 24 & 0xFF; + // + // data[4] = ctrl_word_ & 0xFF; + // data[5] = ctrl_word_ >> 8 & 0xFF; - data[4] = ctrl_word_ & 0xFF; - data[5] = ctrl_word_ >> 8 & 0xFF; - - // data[4] = target_vel_ & 0xFF; - // data[5] = target_vel_ >> 8 & 0xFF; - // data[6] = target_vel_ >> 16 & 0xFF; - // data[7] = target_vel_ >> 24 & 0xFF; + data[4] = target_vel_ & 0xFF; + data[5] = target_vel_ >> 8 & 0xFF; + data[6] = target_vel_ >> 16 & 0xFF; + data[7] = target_vel_ >> 24 & 0xFF; } diff --git a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h index de387cc0..df943618 100644 --- a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h +++ b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h @@ -25,7 +25,7 @@ namespace cmvr { void UpdateData(uint8_t *data) override; int32_t GetLength() const override { - return 0x06; + return 0x08; } uint32_t GetPeriod() const override { diff --git a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index 1c6421c1..e4b26f5e 100644 --- a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -107,6 +107,14 @@ void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { } } +void Ti5MotorCanopenProtocol::set(uint8_t node_id, double angle_rad, double vel) { + auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; + auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; + rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); + rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed))); + can_sender_->Update(rpdo1_commands_[node_id]->ID()); +} + void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { controlword_t cw = {}; @@ -323,7 +331,7 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) { //6 : 映射控制字 seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, - CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32); if (start) { diff --git a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h index b6131327..33cb9fd5 100644 --- a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h @@ -29,6 +29,7 @@ namespace cmvr { bool initNode(uint8_t node_id) override; void setMode(uint8_t node_id, msgs::RunMode mode); + void set(uint8_t node_id, double angle_rad, double vel) override; void setQ(uint8_t node_id, double angle_rad) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 23bc269c..60843950 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -62,7 +62,7 @@ template void HumanoidRobot::torqueOff() { try { for (const auto &pair: motor_manager_->motorsMap()) { - if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P") + if (pair.second->jointName() != "R_WRIST_R" && pair.second->jointName() != "WAIST_P") pair.second->torqueOff(); } } catch (std::exception &e) { @@ -173,137 +173,140 @@ void HumanoidRobot::eStop() { } } -template -void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { - try { - for (const auto &j: cmd) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - // PPM 模式下 这个实际速度会超30% 左右 - motor->setQd(vel); - - if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { - motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - } - motor->setQ(j.rad); - } - } - - //3. wait for completion - bool completion = true; - do { - completion = true; - for (const auto &j: cmd) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - if (!motor->reachedTargetQ()) { - completion = false; - break; - } - } - } - // 4. while waiting, check flash_cmd_, if it is true, set it false then exit - if (flash_cmd_.load()) { - flash_cmd_.store(false); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } while (!completion); - } catch (exception &e) { - throw runtime_error(e.what()); - } -} - - // template // void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { -// if (cmd.empty()) return; +// try { +// for (const auto &j: cmd) { +// auto motor = motor_manager_->getMotor(j.joint_name); +// if (motor != nullptr) { +// // PPM 模式下 这个实际速度会超30% 左右 +// motor->setQd(vel); // -// std::unordered_map cur_joints_angle; -// cur_joints_angle.reserve(cmd.size()); -// -// std::vector > motors; -// motors.reserve(cmd.size()); -// -// for (auto &jp: cmd) { -// auto motor = motor_manager_->getMotor(jp.joint_name); -// motors.push_back(motor); -// // 切 CSP -// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { -// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); -// } -// // 为 map 预填 key -// cur_joints_angle.emplace(jp.joint_name, 0.0); -// } -// -// getJointQ(cur_joints_angle); -// -// std::vector q_start, q_goal; -// q_start.reserve(cmd.size()); -// q_goal.reserve(cmd.size()); -// for (auto &jp: cmd) { -// auto it = cur_joints_angle.find(jp.joint_name); -// if (it == cur_joints_angle.end()) { -// LOG(ERROR) << "missing current angle for " << jp.joint_name; -// return; -// } -// q_start.push_back(it->second); // 当前读到的弧度 -// q_goal.push_back(jp.rad); // 目标弧度 -// } -// if (q_start.size() != q_goal.size()) return; -// -// -// TrajPtr traj; -// joint_space_planner_->setPathType(PathType::Quintic); -// joint_space_planner_->setSymmetricLimits(std::vector(cmd.size(), vel), -// std::vector(cmd.size(), acc)); -// -// -// bool ok = joint_space_planner_->plan(q_start, q_goal, traj); -// if (!ok) { -// LOG(ERROR) << "planner plan() failed"; -// return; -// } -// -// -// // 10ms -// const double dt = 0.01; -// auto samples = joint_space_planner_->sampleTrajectory(traj, dt); -// if (samples.size() < 2) { -// LOG(ERROR) << "trajectory too short"; -// return; -// } -// -// const auto t0 = std::chrono::steady_clock::now(); -// -// // k=0 开始 -// size_t k = 0; -// const size_t K = samples.size(); -// -// while (k < K) { -// // // ——根据墙钟计算“应当在的拍次”，用于追赶—— -// // auto now = std::chrono::steady_clock::now(); -// // double elapsed = std::chrono::duration(now - t0).count(); -// // size_t k_should = static_cast(std::floor(elapsed / dt)); // 向下取整更保守 -// // if (k_should >= K) k_should = K - 1; -// // if (k < k_should) k = k_should; // 追赶，避免越拖越晚 -// -// // ——下发第 k 个采样点—— -// const auto &s = samples[k]; -// -// for (size_t i = 0; i < motors.size(); ++i) { -// motors[i]->setQd(s.qd[i]); -// motors[i]->setQ(s.q[i]); +// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { +// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); +// } +// motor->setQ(j.rad); +// } // } // -// ++k; -// if (k < K) { -// auto next_t = t0 + std::chrono::duration(k * dt); -// std::this_thread::sleep_until(next_t); -// } +// //3. wait for completion +// bool completion = true; +// do { +// completion = true; +// for (const auto &j: cmd) { +// auto motor = motor_manager_->getMotor(j.joint_name); +// if (motor != nullptr) { +// if (!motor->reachedTargetQ()) { +// completion = false; +// break; +// } +// } +// } +// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit +// if (flash_cmd_.load()) { +// flash_cmd_.store(false); +// return; +// } +// std::this_thread::sleep_for(std::chrono::milliseconds(2)); +// } while (!completion); +// } catch (exception &e) { +// throw runtime_error(e.what()); // } // } + +template +void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { + if (cmd.empty()) return; + + LOG(INFO) << "vel " << vel << " acc" << acc << " " << cmd.size() << endl; + + std::unordered_map cur_joints_angle; + cur_joints_angle.reserve(cmd.size()); + + std::vector > motors; + motors.reserve(cmd.size()); + + for (auto &jp: cmd) { + auto motor = motor_manager_->getMotor(jp.joint_name); + motors.push_back(motor); + // 切 CSP + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + // 为 map 预填 key + cur_joints_angle.emplace(jp.joint_name, 0.0); + } + + getJointQ(cur_joints_angle); + + std::vector q_start, q_goal; + q_start.reserve(cmd.size()); + q_goal.reserve(cmd.size()); + for (auto &jp: cmd) { + auto it = cur_joints_angle.find(jp.joint_name); + if (it == cur_joints_angle.end()) { + LOG(ERROR) << "missing current angle for " << jp.joint_name; + return; + } + q_start.push_back(it->second); // 当前读到的弧度 + q_goal.push_back(jp.rad); // 目标弧度 + } + if (q_start.size() != q_goal.size()) return; + + + TrajPtr traj; + joint_space_planner_->setPathType(PathType::Quintic); + joint_space_planner_->setSymmetricLimits(std::vector(cmd.size(), vel), + std::vector(cmd.size(), acc)); + + + bool ok = joint_space_planner_->plan(q_start, q_goal, traj); + if (!ok) { + LOG(ERROR) << "planner plan() failed"; + return; + } + + + // 10ms + const double dt = 0.001; + auto samples = joint_space_planner_->sampleTrajectory(traj, dt); + if (samples.size() < 2) { + LOG(ERROR) << "trajectory too short"; + return; + } + + const auto t0 = std::chrono::steady_clock::now(); + + // k=0 开始 + size_t k = 0; + const size_t K = samples.size(); + + while (k < K) { + // // ——根据墙钟计算“应当在的拍次”，用于追赶—— + // auto now = std::chrono::steady_clock::now(); + // double elapsed = std::chrono::duration(now - t0).count(); + // size_t k_should = static_cast(std::floor(elapsed / dt)); // 向下取整更保守 + // if (k_should >= K) k_should = K - 1; + // if (k < k_should) k = k_should; // 追赶，避免越拖越晚 + + // ——下发第 k 个采样点—— + const auto &s = samples[k]; + + for (size_t i = 0; i < motors.size(); ++i) { + // motors[i]->setQd(s.qd[i]); + // motors[i]->setQ(s.q[i]); + motors[i]->set(s.q[i],s.qd[i]); + } + + ++k; + if (k <= K) { + auto next_t = t0 + std::chrono::duration(k * dt); + std::this_thread::sleep_until(next_t); + } + } +} + template void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { auto motor = motor_manager_->getMotor(joint_name); diff --git a/src/main.cpp b/src/main.cpp index 4aff85a2..61363de8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,7 +21,7 @@ #include "http/httpclient.h" #include "json/json.h" - +#include void runServer(const XmlNode &cfg){ using namespace cmvr::device; using namespace cmvr::service; @@ -32,6 +32,8 @@ void runServer(const XmlNode &cfg){ return; } try { + // 🔥 关键！启用反射 + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); auto dmgr_cfg = cfg.getChild("DeviceManager"); DeviceManager::getInstance(dmgr_cfg); diff --git a/src/service/grpc_head_service.cpp b/src/service/grpc_head_service.cpp index 9e7c9ecb..d6d3aa73 100644 --- a/src/service/grpc_head_service.cpp +++ b/src/service/grpc_head_service.cpp @@ -227,3 +227,126 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( return grpc::Status::OK; } } + +grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_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->speakstart(); // kaish开始 + + 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::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_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->speakstop(); // 停止执行 + + 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::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_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->expressionHappy(); // 停止执行 + + 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::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_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->expressionSurprised(); // + + 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::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_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->expressionTired(); // 停止执行 + + 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; + } +}