From 9fb185b5a89d88f77a09fbe2ef041ed1ef22b30c Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Mon, 11 May 2026 16:49:22 +0800 Subject: [PATCH] update proto config --- .../src/touch_screen_app_test.cpp | 2 +- .../biohead_config/biohead_config.pb.txt | 27 + cmvr-es/common/config/cabin_robot.xml | 152 ---- .../config/camera_config/camera_config.pb.txt | 2 +- .../dexhand_config/dexhand_config.pb.txt | 4 +- .../monitor_config/monitor_config.pb.txt | 25 + .../config/robot_config/robot_config.pb.txt | 212 ++++++ .../config/server_config/server_config.pb.txt | 3 + .../config_helper/include/config_helper.h | 17 + .../config_helper/include/config_setting.h | 3 + .../config_helper/src/config_setting.cpp | 12 + cmvr-es/data_center/CMakeLists.txt | 1 + cmvr-es/data_center/include/motors_info.h | 4 +- cmvr-es/data_center/src/motors_info.cpp | 155 +++-- .../device_manager/include/device_factory.h | 10 +- .../device_manager/include/device_manager.h | 3 +- cmvr-es/device_manager/src/device_factory.cpp | 197 +----- cmvr-es/device_manager/src/device_manager.cpp | 145 ++-- cmvr-es/devices/biohead/abstract_biohead.h | 1 + .../biohead_esp32/include/biohead_esp32.h | 5 +- .../biohead_esp32/src/biohead_esp32.cpp | 213 ++++++ .../src/realsense_camera_test.cpp | 2 +- cmvr-es/devices/canbus/abstract_canbus.h | 1 + .../socket/socket_can_client_raw.cc | 11 + .../can_client/socket/socket_can_client_raw.h | 4 + cmvr-es/devices/motor/CMakeLists.txt | 2 +- cmvr-es/devices/motor/abstract_motor.h | 1 + .../devices/motor/eyou_motor/CMakeLists.txt | 19 + .../ethercat/eyou_motor_ethercat_protocol.cpp | 652 ++++++++++++++++++ .../ethercat/eyou_motor_ethercat_protocol.h | 106 +++ .../devices/motor/eyou_motor/eyou_motor.cpp | 5 + cmvr-es/devices/motor/eyou_motor/eyou_motor.h | 51 ++ .../devices/motor/motor_protocol_interface.h | 3 +- cmvr-es/devices/motor/ti5_motor/ti5_motor.h | 12 +- cmvr-es/devices/robot/abstract_robot.h | 1 + .../humanoid_robot/include/humanoid_robot.h | 9 +- .../humanoid_robot/src/humanoid_robot.cpp | 85 ++- .../src/humanoid_robot_test.cpp | 28 +- cmvr-es/main.cpp | 24 +- cmvr-es/monitor/abstract_monitor.h | 1 + .../diskmonitor/include/disk_monitor.h | 6 +- .../monitor/diskmonitor/src/disk_monitor.cpp | 64 +- .../monitor_manager/include/monitor_factory.h | 15 +- .../monitor_manager/include/monitor_manager.h | 25 +- .../monitor_manager/src/monitor_factory.cpp | 28 +- .../monitor_manager/src/monitor_manager.cpp | 56 +- cmvr-es/service/grpc/include/server_runner.h | 7 +- .../grpc/src/grpc_humanoid_robot_service.cpp | 2 +- cmvr-es/service/grpc/src/server_runner.cpp | 26 +- .../biohead_config/biohead_config.proto | 44 ++ .../monitor_config/monitor_config.proto | 22 + .../config/robot_config/robot_config.proto | 88 +++ .../config/server_config/server_config.proto | 10 + request.txt | 1 + 54 files changed, 2023 insertions(+), 581 deletions(-) create mode 100644 cmvr-es/common/config/biohead_config/biohead_config.pb.txt create mode 100644 cmvr-es/common/config/monitor_config/monitor_config.pb.txt create mode 100644 cmvr-es/common/config/robot_config/robot_config.pb.txt create mode 100644 cmvr-es/common/config/server_config/server_config.pb.txt create mode 100644 cmvr-es/devices/motor/eyou_motor/CMakeLists.txt create mode 100644 cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp create mode 100644 cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h create mode 100644 cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp create mode 100644 cmvr-es/devices/motor/eyou_motor/eyou_motor.h create mode 100644 protos/cmvr/config/biohead_config/biohead_config.proto create mode 100644 protos/cmvr/config/monitor_config/monitor_config.proto create mode 100644 protos/cmvr/config/robot_config/robot_config.proto create mode 100644 protos/cmvr/config/server_config/server_config.proto diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp index b2c178fe..08aa560e 100644 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -21,7 +21,7 @@ void run_touch_once(int u, int v) { const XmlNode config(kConfigPath); // auto& dm = cmvr::device::DeviceManager::getInstance(); - cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager")); + cmvr::device::DeviceManager::getInstance(); // cmvr::service::ServerRunner runner; diff --git a/cmvr-es/common/config/biohead_config/biohead_config.pb.txt b/cmvr-es/common/config/biohead_config/biohead_config.pb.txt new file mode 100644 index 00000000..b2880e64 --- /dev/null +++ b/cmvr-es/common/config/biohead_config/biohead_config.pb.txt @@ -0,0 +1,27 @@ +bioheads { + id: "bio_head" + serial: "/dev/ttyUSB0" + ctrlFreq: 50 + enable: false + + eyebrow { + serial: "64:0~3" + offest: "90 90 90 90" + jLmtUp: "90 170 155 110" + jLmtLow: "20 77 90 20" + } + + eye { + serial: "64:4~9" + offest: "90 90 90 90 90 90" + jLmtUp: "90 150 165 90 120 115" + jLmtLow: "20 90 90 25 70 75" + } + + mouth { + serial: "65:0~9" + offest: "90 90 90 90 90 90 90 90 90 90" + jLmtUp: "150 110 130 140 100 105 110 125 90 95" + jLmtLow: "70 30 80 80 65 55 45 80 85 90" + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index d67dffac..e69de29b 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -1,152 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt index 18b02c30..f35f4018 100644 --- a/cmvr-es/common/config/camera_config/camera_config.pb.txt +++ b/cmvr-es/common/config/camera_config/camera_config.pb.txt @@ -27,7 +27,7 @@ realsense_cameras { align_mode: ALIGN_MODE_COLOR buffer_size: 30 sync: false - enable: true + enable: false } realsense_cameras { diff --git a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt index e6783dd2..6adf262b 100644 --- a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt +++ b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt @@ -1,6 +1,6 @@ rh56dftp_dexhands { id: "hand1" - ip: "192.168.1.213" + ip: "192.168.1.223" port: 6000 poll_interval_ms: 10 enable: false @@ -11,7 +11,7 @@ rh56dftp_dexhands { ip: "192.168.1.224" port: 6000 poll_interval_ms: 10 - enable: true + enable: false } px_6ax_gen3 { diff --git a/cmvr-es/common/config/monitor_config/monitor_config.pb.txt b/cmvr-es/common/config/monitor_config/monitor_config.pb.txt new file mode 100644 index 00000000..d5350f3c --- /dev/null +++ b/cmvr-es/common/config/monitor_config/monitor_config.pb.txt @@ -0,0 +1,25 @@ +# 监控管理器配置 +monitor_manager { + disk_monitor { + id: "file_monitor" + freq: 1 + enable: false + + folders { + fileDir: "/home/share/assets/audio" + maxVolume: 1000 + } + folders { + fileDir: "/home/share/assets/image" + maxVolume: 1000 + } + folders { + fileDir: "/home/share/assets/video" + maxVolume: 1000 + } + folders { + fileDir: "../log" + maxVolume: 1000 + } + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/robot_config/robot_config.pb.txt b/cmvr-es/common/config/robot_config/robot_config.pb.txt new file mode 100644 index 00000000..cfbd4cd5 --- /dev/null +++ b/cmvr-es/common/config/robot_config/robot_config.pb.txt @@ -0,0 +1,212 @@ +# HumanRobot配置 +humanrobots { + id: "hc01" + dof: 14 + urdf: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + baseLink: "PELVIS_S" + jointNames: "L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R" + linkNames: "PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM" + bufferSize: 50 + verbose: false + enable: false + + can_manager { + id: "" + devId: "" + + left_arm { + id: "" + devId: "" + channelId: 0 + enable: false + toolFrame: "L_FINGER_TIP" + motors { + id: 23 + jointName: "L_SHOULDER_P" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 24 + jointName: "L_SHOULDER_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 25 + jointName: "L_SHOULDER_Y" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 26 + jointName: "L_ELBOW_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 27 + jointName: "L_WRIST_P" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 28 + jointName: "L_WRIST_Y" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 29 + jointName: "L_WRIST_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + } + + right_arm { + id: "" + devId: "" + channelId: 1 + enable: true + toolFrame: "R_FINGER_TIP" + motors { + id: 16 + jointName: "R_SHOULDER_P" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 17 + jointName: "R_SHOULDER_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 18 + jointName: "R_SHOULDER_Y" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 19 + jointName: "R_ELBOW_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 20 + jointName: "R_WRIST_P" + limitQLb: -3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 21 + jointName: "R_WRIST_Y" + limitQLb: -1.102 + limitQUb: 1.02 + limitQd: 3.0 + } + motors { + id: 22 + jointName: "R_WRIST_R" + limitQLb: -0.293 + limitQUb: 1.57079 + limitQd: 3.0 + } + } + + head { + id: "" + devId: "" + channelId: 2 + enable: true + motors { + id: 32 + jointName: "HEAD_Y" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 30 + jointName: "HEAD_P" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 31 + jointName: "HEAD_R" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + } + + waist { + id: "" + devId: "" + channelId: 3 + enable: false + motors { + id: 4 + jointName: "WAIST_Y" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + motors { + id: 15 + jointName: "WAIST_P" + limitQLb: 3.14 + limitQUb: 3.14 + limitQd: 3.0 + } + } + } +} + + + +# EtherCAT机器人配置 +ethercat_robots { + id: "ec_robot_01" + dof: 6 + urdf: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + baseLink: "PELVIS_S" + jointNames: "L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R" + linkNames: "PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM" + bufferSize: 100 + verbose: true + updFreq: 1000 + enable: true + + # EtherCAT主站配置 + master { + interface: "enp1s0" + cycle_time_us: 1000 + enable: true + } + + # 电机配置列表 + motors { + slave: 1 + jointName: "L_SHOULDER_P" + limitQLb: -3.14 + limitQUb: 3.14 + limitQd: 3.0 + } +} \ No newline at end of file diff --git a/cmvr-es/common/config/server_config/server_config.pb.txt b/cmvr-es/common/config/server_config/server_config.pb.txt new file mode 100644 index 00000000..78599a06 --- /dev/null +++ b/cmvr-es/common/config/server_config/server_config.pb.txt @@ -0,0 +1,3 @@ +grpc_server { + port: "50052" +} \ No newline at end of file diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h index 7d189128..445af379 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/cmvr-es/common/utils/config_helper/include/config_helper.h @@ -10,6 +10,9 @@ #include "cmvr/config/microphone_config/microphone_config.pb.h" #include "cmvr/config/speaker_config/speaker_conifg.pb.h" #include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" +#include "cmvr/config/biohead_config/biohead_config.pb.h" +#include "cmvr/config/robot_config/robot_config.pb.h" +#include "cmvr/config/server_config/server_config.pb.h" #define GET_CONFIG(file, para) \ ([&]() -> bool { \ @@ -82,6 +85,20 @@ namespace cmvr return SET_CONFIG(config, touch_screen_app_config_file); } + static bool getHeadConfig(config::HeadConfig& config) + { + return GET_CONFIG(biohead_config_file, config); + } + + static bool getRobotConfig(config::RobotConfig& config) + { + return GET_CONFIG(robot_config_file, config); + } + + static bool getServerConfig(config::ServerConfig& config) + { + return GET_CONFIG(server_config_file, config); + } private: // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx) template diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h index 5d58fb9e..caedf533 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/cmvr-es/common/utils/config_helper/include/config_setting.h @@ -6,3 +6,6 @@ DECLARE_string(dexhand_config_file); DECLARE_string(microphone_config_file); DECLARE_string(speaker_config_file); DECLARE_string(touch_screen_app_config_file); +DECLARE_string(biohead_config_file); +DECLARE_string(robot_config_file); +DECLARE_string(server_config_file); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 9e44092d..528819f5 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -88,3 +88,15 @@ DEFINE_string(speaker_config_file, DEFINE_string(touch_screen_app_config_file, basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt", "The configuration file for TouchScreenApp"); + +DEFINE_string(biohead_config_file, + basePath() + "biohead_config/biohead_config.pb.txt", + "The configuration file for BioHead"); + +DEFINE_string(robot_config_file, + basePath() + "robot_config/robot_config.pb.txt", + "The configuration file for Robot"); + +DEFINE_string(server_config_file, + basePath() + "server_config/server_config.pb.txt", + "The configuration file for Server"); diff --git a/cmvr-es/data_center/CMakeLists.txt b/cmvr-es/data_center/CMakeLists.txt index d0fda580..ff66818d 100644 --- a/cmvr-es/data_center/CMakeLists.txt +++ b/cmvr-es/data_center/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(data_center PRIVATE cmvr_es::utils cmvr_es::device::canbus cmvr_es::device::ti5motor + cmvr_es::device::eyoumotor protobuf glog ) diff --git a/cmvr-es/data_center/include/motors_info.h b/cmvr-es/data_center/include/motors_info.h index 56c2de04..4c86ce00 100644 --- a/cmvr-es/data_center/include/motors_info.h +++ b/cmvr-es/data_center/include/motors_info.h @@ -23,8 +23,8 @@ namespace cmvr { public: ~MotorsInfo(){}; - - void init(const XmlNode &cfg); + void init(const cmvr::config::HumanRobotConfig& config); + void init(const cmvr::config::EthercatRobotConfig& config); static MotorsInfo *getInstance() { static MotorsInfo instance; diff --git a/cmvr-es/data_center/src/motors_info.cpp b/cmvr-es/data_center/src/motors_info.cpp index 4b6a087c..c1eafea6 100644 --- a/cmvr-es/data_center/src/motors_info.cpp +++ b/cmvr-es/data_center/src/motors_info.cpp @@ -3,6 +3,8 @@ // #include "data_center/include/motors_info.h" + +#include "motor/eyou_motor/eyou_motor.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" #include "motor/ti5_motor/ti5_motor.h" @@ -12,99 +14,95 @@ using namespace cmvr::device; MotorsInfo::MotorsInfo() { } -void MotorsInfo::init(const XmlNode &cfg) { - // 定义 lambda 函数，用于读取 XML 节点 enable 属性 - auto readEnable = [](const XmlNode &node) -> bool { - std::string enable_str = node.getAttrString("enable"); // 默认 false - return (enable_str == "true" || enable_str == "1"); - }; - auto can_cfg = cfg.getChild("CanManger"); +void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) { + const auto& can_manager = config.can_manager(); - auto l_can_cfg = can_cfg.getChild("LeftArmCan"); - left_arm_enabled_ = readEnable(l_can_cfg); + // 读取enable状态 + left_arm_enabled_ = can_manager.left_arm().enable(); + right_arm_enabled_ = can_manager.right_arm().enable(); + waist_enabled_ = can_manager.waist().enable(); + head_enabled_ = can_manager.head().enable(); + // 局部变量存储电机配置 + std::vector l_motors_cfg; + std::vector r_motors_cfg; + std::vector waist_motors_cfg; + std::vector head_motors_cfg; + + // 左臂 if (left_arm_enabled_) { - l_motors_cfg_ = l_can_cfg.getChildren("Motor"); - l_can_client_ = std::make_shared(l_can_cfg); - l_can_sender_ = std::make_shared >(); - l_can_receiver_ = std::make_shared >(); - l_message_manager_ = std::make_shared >(); + const auto& left_arm = can_manager.left_arm(); + for (const auto& motor : left_arm.motors()) { + l_motors_cfg.push_back(motor); + } + l_can_client_ = std::make_shared(left_arm); + l_can_sender_ = std::make_shared>(); + l_can_receiver_ = std::make_shared>(); + l_message_manager_ = std::make_shared>(); } - - auto r_can_cfg = can_cfg.getChild("RightArmCan"); - right_arm_enabled_ = readEnable(r_can_cfg); + // 右臂 if (right_arm_enabled_) { - r_motors_cfg_ = r_can_cfg.getChildren("Motor"); - r_can_client_ = std::make_shared(r_can_cfg); - r_can_sender_ = std::make_shared >(); - r_can_receiver_ = std::make_shared >(); - r_message_manager_ = std::make_shared >(); + const auto& right_arm = can_manager.right_arm(); + for (const auto& motor : right_arm.motors()) { + r_motors_cfg.push_back(motor); + } + r_can_client_ = std::make_shared(right_arm); + r_can_sender_ = std::make_shared>(); + r_can_receiver_ = std::make_shared>(); + r_message_manager_ = std::make_shared>(); } - - auto waist_can_cfg = can_cfg.getChild("WaistCan"); - waist_enabled_ = readEnable(waist_can_cfg); + // 腰部 if (waist_enabled_) { - waist_motors_cfg_ = waist_can_cfg.getChildren("Motor"); - waist_can_client_ = std::make_shared(waist_can_cfg); - waist_can_sender_ = std::make_shared >(); - waist_can_receiver_ = std::make_shared >(); - waist_message_manager_ = std::make_shared >(); + const auto& waist = can_manager.waist(); + for (const auto& motor : waist.motors()) { + waist_motors_cfg.push_back(motor); + } + waist_can_client_ = std::make_shared(waist); + waist_can_sender_ = std::make_shared>(); + waist_can_receiver_ = std::make_shared>(); + waist_message_manager_ = std::make_shared>(); } - auto head_can_cfg = can_cfg.getChild("HeadCan"); - head_enabled_ = readEnable(head_can_cfg); + // 头部 if (head_enabled_) { - head_motors_cfg_ = head_can_cfg.getChildren("Motor"); - head_can_client_ = std::make_shared(head_can_cfg); - head_can_sender_ = std::make_shared >(); - head_can_receiver_ = std::make_shared >(); - head_message_manager_ = std::make_shared >(); + const auto& head = can_manager.head(); + for (const auto& motor : head.motors()) { + head_motors_cfg.push_back(motor); + } + head_can_client_ = std::make_shared(head); + head_can_sender_ = std::make_shared>(); + head_can_receiver_ = std::make_shared>(); + head_message_manager_ = std::make_shared>(); } - // 开始初始化 + // 初始化 limbs struct Limb { std::string name; bool enabled; std::shared_ptr client; - std::shared_ptr > sender; - std::shared_ptr > receiver; - std::shared_ptr > message_manager; - std::vector motor_cfgs; + std::shared_ptr> sender; + std::shared_ptr> receiver; + std::shared_ptr> message_manager; + std::vector motor_cfgs; }; std::vector limbs{ - { - "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, - waist_motors_cfg_ - }, - { - "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, - l_motors_cfg_ - }, - { - "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, - r_motors_cfg_ - }, - { - "HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, - head_motors_cfg_ - } + {"WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, waist_motors_cfg}, + {"LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, l_motors_cfg}, + {"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, r_motors_cfg}, + {"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, head_motors_cfg} }; - // 创建 MotorManager motor_manager_ = std::make_shared(); + std::vector> tasks; - std::vector > tasks; - - for (auto &limb: limbs) { + for (auto &limb : limbs) { if (!limb.enabled) continue; - if (limb.client) limb.client->init(); - // 2. 初始化 Sender / Receiver（如果有） if (limb.sender && limb.receiver && limb.client) { auto ret = limb.sender->Init(limb.client.get(), false); if (ret != ErrorCode::OK) @@ -124,19 +122,16 @@ void MotorsInfo::init(const XmlNode &cfg) { LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver."; } - // 3. 创建协议（如果有CAN） std::shared_ptr protocol = nullptr; if (limb.sender && limb.message_manager) { protocol = std::make_shared(limb.sender, limb.message_manager); } - // 4. 并行初始化电机 if (!limb.motor_cfgs.empty()) { tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] { - LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << - " motors..."; - for (const auto &cfg: limb.motor_cfgs) { - auto motor = std::make_shared(cfg); + LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << " motors..."; + for (const auto& motor_cfg : limb.motor_cfgs) { + auto motor = std::make_shared(motor_cfg); if (protocol) motor->setProtocol(protocol); motor->init(); motor_manager_->addMotor(motor); @@ -145,12 +140,26 @@ void MotorsInfo::init(const XmlNode &cfg) { } } - // 等待所有任务完成 - for (auto &task: tasks) task.get(); - + for (auto& task : tasks) task.get(); LOG(INFO) << "All enabled motors initialized successfully."; } + +void MotorsInfo::init(const cmvr::config::EthercatRobotConfig& config) +{ + if (!motor_manager_) + { + motor_manager_ = std::make_shared(); + } + auto protocol = std::make_shared(config.master().interface()); + for (const auto& motor_cfg : config.motors()) { + auto motor = std::make_shared(motor_cfg); + if (protocol) motor->setProtocol(protocol); + motor->init(); + motor_manager_->addMotor(motor); + } +} + bool MotorsInfo::getJointsQ(const std::vector &joints_name, std::unordered_map &joint_qs) const { auto res{true}; diff --git a/cmvr-es/device_manager/include/device_factory.h b/cmvr-es/device_manager/include/device_factory.h index 1fcce739..096664a0 100644 --- a/cmvr-es/device_manager/include/device_factory.h +++ b/cmvr-es/device_manager/include/device_factory.h @@ -28,15 +28,7 @@ namespace cmvr::device { template std::shared_ptr createFromConfig(const ConfigType& cfg); private: - std::shared_ptr create_agv_(const XmlNode& cfg); - std::shared_ptr create_battery_(const XmlNode& cfg); - std::shared_ptr create_camera_(const XmlNode& cfg); - std::shared_ptr create_dexhand_(const XmlNode& cfg); - std::shared_ptr create_gripper_(const XmlNode& cfg); - std::shared_ptr create_mic_(const XmlNode& cfg); - std::shared_ptr create_robot_(const XmlNode& cfg); - std::shared_ptr create_speaker_(const XmlNode& cfg); - std::shared_ptr create_biohead_(const XmlNode& cfg); + }; } diff --git a/cmvr-es/device_manager/include/device_manager.h b/cmvr-es/device_manager/include/device_manager.h index ee782e5d..865d1104 100644 --- a/cmvr-es/device_manager/include/device_manager.h +++ b/cmvr-es/device_manager/include/device_manager.h @@ -41,7 +41,6 @@ namespace cmvr::device { DeviceManager(const DeviceManager&) = delete; DeviceManager& operator=(const DeviceManager&) = delete; - static DeviceManager& getInstance(const XmlNode &cfg); static DeviceManager& getInstance(); static void destroyInstance(); @@ -68,7 +67,7 @@ namespace cmvr::device { std::unique_ptr dev_factory_; std::unique_ptr sys_monitor_; - explicit DeviceManager(const XmlNode &cfg); + explicit DeviceManager(); void init_devices_(); void get_os_info_(); }; diff --git a/cmvr-es/device_manager/src/device_factory.cpp b/cmvr-es/device_manager/src/device_factory.cpp index fba92e51..f80e53f4 100644 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ b/cmvr-es/device_manager/src/device_factory.cpp @@ -18,172 +18,16 @@ //#include "devices/robot/aubo_robot/include/aubo_robot.h" using namespace std; using namespace cmvr::device; - - -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); -template std::shared_ptr DeviceFactory::create(const XmlNode&); - -template -std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { - if constexpr (std::is_same_v) { - return create_agv_(cfg); - } else if constexpr (std::is_same_v) { - return create_battery_(cfg); - } else if constexpr (std::is_same_v) { - return create_camera_(cfg); - } else if constexpr (std::is_same_v) { - return create_dexhand_(cfg); - } else if constexpr (std::is_same_v) { - return create_gripper_(cfg); - } else if constexpr (std::is_same_v) { - return create_mic_(cfg); - } else if constexpr (std::is_same_v) { - return create_robot_(cfg); - } else if constexpr (std::is_same_v) { - return create_speaker_(cfg); - }else if constexpr (std::is_same_v){ - return create_biohead_(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); - } -} - -std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_battery_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_camera_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "UVCCamera") { - return std::make_shared(cfg); - } - else if (cfg.getNodeName() == "MechMind") { - //return std::make_shared(cfg); - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - else if (cfg.getNodeName() == "RealsenseCamera") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_dexhand_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "RH56DFTP") { - return std::make_shared(cfg); - } - else if (cfg.getNodeName() == "PX6AXGen3") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_gripper_(const XmlNode& cfg) { - return nullptr; -} - -std::shared_ptr DeviceFactory::create_mic_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "ffmpegMicPhone") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } - return nullptr; -} - -std::shared_ptr DeviceFactory::create_robot_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "Humanoid") { - auto motos_info = MotorsInfo::getInstance(); - motos_info->init(cfg); - return std::make_shared>(cfg); - } - else if (cfg.getNodeName() == "AuboRobot") { - //return std::make_shared>(cfg); - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptrDeviceFactory::create_biohead_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "esp32") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - -std::shared_ptr DeviceFactory::create_speaker_(const XmlNode& cfg) { - try { - if (cfg.getNodeName() == "ffmpegSpeaker") { - return std::make_shared(cfg); - } - else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName(); - return nullptr; - } - } - catch (const exception &e) { - return nullptr; - } -} - template std::shared_ptr DeviceFactory::createFromConfig(const config::UVCCameraConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::RealSenseCameraConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::RH56DFTPDexHandConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::PX6AXGen3& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegMicroPhoneConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegSpeakerConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::BioHeadConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::HumanRobotConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::EthercatRobotConfig& cfg); + template std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cfg) { @@ -244,6 +88,38 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); } } + else if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + return std::make_shared(cfg); + } + else + { + LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; + throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); + } + } + else if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + auto motos_info = MotorsInfo::getInstance(); + motos_info->init(cfg); + return std::make_shared>(cfg); + } + else if constexpr (std::is_same_v) + { + auto motos_info = MotorsInfo::getInstance(); + motos_info->init(cfg); + return std::make_shared>(cfg); + } + else + { + LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; + throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); + } + } else { LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; throw runtime_error("[DeviceFactory]: Unsupported device type"); @@ -253,3 +129,4 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf return nullptr; } } + diff --git a/cmvr-es/device_manager/src/device_manager.cpp b/cmvr-es/device_manager/src/device_manager.cpp index 805516f9..93ff9534 100644 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ b/cmvr-es/device_manager/src/device_manager.cpp @@ -24,12 +24,9 @@ std::shared_ptr DeviceManager::instance_ = nullptr; std::once_flag DeviceManager::init_flag_; -DeviceManager::DeviceManager(const XmlNode& cfg) { +DeviceManager::DeviceManager() { try { - cfg_ = cfg; - info_.version = cfg_.getAttrDefault("ver", "1.0"); - info_.name = cfg_.getAttrDefault("name", "cmvr_es"); - info_.description = cfg_.getAttrDefault("description", "empty"); + get_os_info_(); dev_factory_ = std::make_unique(); @@ -42,17 +39,12 @@ DeviceManager::DeviceManager(const XmlNode& cfg) { } } -DeviceManager& DeviceManager::getInstance(const XmlNode& cfg) { - std::call_once(init_flag_, [&cfg] { - instance_.reset(new DeviceManager(cfg)); - }); - return *instance_; -} - DeviceManager& DeviceManager::getInstance() { - if (!instance_) { - throw std::runtime_error("[DeviceManager] (getInstance): DeviceManager not initialized. Call getInstance(const XmlNode&) first."); - } + if (!instance_) + std::call_once(init_flag_, [&] { + instance_.reset(new DeviceManager()); + }); + return *instance_; } @@ -199,38 +191,38 @@ void DeviceManager::getSystemStatus(SystemStatus& status) const { void DeviceManager::init_devices_() { try{ - auto dmgr_node = cfg_.getChild("Devices"); - auto battery_node = dmgr_node.getChild("Battery"); - for (auto &node: battery_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id; - throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success"; - } + // auto dmgr_node = cfg_.getChild("Devices"); + // auto battery_node = dmgr_node.getChild("Battery"); + // for (auto &node: battery_node.getChildren()){ + // string id = node.getAttrString("id"); + // if (devices_.count(id)) { + // LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id; + // throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id); + // } + // auto device = dev_factory_->create(node); + // if (device == nullptr) { + // throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + // } + // devices_[id] = device; + // std::get>(devices_[id])->init(); + // LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success"; + // } - auto agv_node = dmgr_node.getChild("AGV"); - for (auto &node: agv_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); - } - devices_[id] = device; - std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success"; - } + // auto agv_node = dmgr_node.getChild("AGV"); + // for (auto &node: agv_node.getChildren()){ + // string id = node.getAttrString("id"); + // if (devices_.count(id)) { + // LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id; + // throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id); + // } + // auto device = dev_factory_->create(node); + // if (device == nullptr) { + // throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + // } + // devices_[id] = device; + // std::get>(devices_[id])->init(); + // LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success"; + // } config::CameraConfig camera_config; ConfigHelper::getCamerasConfig(camera_config); @@ -296,20 +288,36 @@ void DeviceManager::init_devices_() { std::get>(devices_[id])->init(); } - auto robot_node = dmgr_node.getChild("Robot"); - for (auto &node: robot_node.getChildren()){ - string id = node.getAttrString("id"); + config::RobotConfig robot_config; + ConfigHelper::getRobotConfig(robot_config); + for (int i = 0; i < robot_config.humanrobots().size(); i++) { + auto robot = robot_config.humanrobots(i); + if (!robot.enable()) + continue; + auto id = robot.id(); if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate robot Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate robot Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id); } + std::cout << "DexHand[" << i << "]: " << id << std::endl; + auto device = dev_factory_->createFromConfig(robot); + devices_[id] = device; + std::get>(devices_[id])->init(); + } + + for (int i = 0; i < robot_config.ethercat_robots().size(); i++) { + auto robot = robot_config.ethercat_robots(i); + if (!robot.enable()) + continue; + auto id = robot.id(); + if (devices_.count(id)) { + LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id); + } + std::cout << "DexHand[" << i << "]: " << id << std::endl; + auto device = dev_factory_->createFromConfig(robot); devices_[id] = device; std::get>(devices_[id])->init(); - LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success"; } config::MicroPhoneConfig micro_phone_config; @@ -346,22 +354,21 @@ void DeviceManager::init_devices_() { std::get>(devices_[id])->init(); } - - auto biohead_node = dmgr_node.getChild("BioHead"); - for (auto &node: biohead_node.getChildren()){ - string id = node.getAttrString("id"); - if (devices_.count(id)){ - LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id); - } - auto device = dev_factory_->create(node); - if (device == nullptr){ - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + config::HeadConfig head_cfg; + ConfigHelper::getHeadConfig(head_cfg); + for (int i = 0; i < head_cfg.bioheads().size(); i++) { + auto head = head_cfg.bioheads(i); + if (!head.enable()) + continue; + auto id = head.id(); + if (devices_.count(id)) { + LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id); } + std::cout << "Biohead[" << i << "]: " << id << std::endl; + auto device = dev_factory_->createFromConfig(head); devices_[id] = device; std::get>(devices_[id])->init(); - LOG(INFO) << "[BioHead]: Init biohead " << id << " Success"; - } } catch (const exception& e) { diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index 93e0eb20..09b774ff 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -58,6 +58,7 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: + AbstractBiohead() = default; explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {} ~AbstractBiohead() override = default; diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 52b34d5d..08c0953f 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -7,7 +7,7 @@ #include #include #include - +#include "cmvr/config/biohead_config/biohead_config.pb.h" namespace cmvr::device { struct ServoChannel { @@ -18,6 +18,7 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const XmlNode &config); + BioHeadRobot(const cmvr::config::BioHeadConfig& config); ~BioHeadRobot() override = default; // 接口实现 @@ -66,7 +67,7 @@ namespace cmvr::device { std::shared_ptr speak_thread_; std::atomic speak_running_{false}; - + cmvr::config::BioHeadConfig config_; }; diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index c3b67887..8ff9e3b3 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -26,6 +26,219 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) { } +BioHeadRobot::BioHeadRobot(const cmvr::config::BioHeadConfig& config) : config_(config) { + // 从config中读取串口参数 + port_name_ = config_.serial(); + + serial_ = std::make_shared(); + if (!serial_->open(port_name_, 115200)) { + LOG(ERROR) << "[BioHeadRobot] Failed to open serial port: " << port_name_; + } else { + serial_->wakeupESP32(port_name_); + LOG(INFO) << "[BioHeadRobot] Serial port opened and ESP32 woken up."; + } + + // 解析眉毛配置 + if (config_.has_eyebrow()) { + const auto& eyebrow = config_.eyebrow(); + std::string serial_str = eyebrow.serial(); + std::string offset_str = eyebrow.offest(); + std::string min_angles_str = eyebrow.jlmtlow(); + std::string max_angles_str = eyebrow.jlmtup(); + + // 解析偏移角 + std::vector offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector min_angles; + if (!min_angles_str.empty()) { + std::istringstream min_ss(min_angles_str); + while (min_ss >> val) min_angles.push_back(val); + } + + std::vector max_angles; + if (!max_angles_str.empty()) { + std::istringstream max_ss(max_angles_str); + while (max_ss >> val) max_angles.push_back(val); + } + + // 解析 channel，如 64:0~3 + size_t colon = serial_str.find(':'); + if (colon != std::string::npos) { + uint8_t addr = std::stoi(serial_str.substr(0, colon)); + std::string range = serial_str.substr(colon + 1); + size_t tilde = range.find('~'); + if (tilde != std::string::npos) { + int ch_start = std::stoi(range.substr(0, tilde)); + int ch_end = std::stoi(range.substr(tilde + 1)); + int channel_count = ch_end - ch_start + 1; + + // 添加舵机通道 + for (int ch = ch_start; ch <= ch_end; ++ch) { + channels_.push_back({addr, static_cast(ch)}); + } + + // 添加角度限制 + if (min_angles.size() == 1) { + min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); + } else if (min_angles.size() >= channel_count) { + min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); + } else { + min_angles_.insert(min_angles_.end(), channel_count, 0.0); + } + + if (max_angles.size() == 1) { + max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); + } else if (max_angles.size() >= channel_count) { + max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); + } else { + max_angles_.insert(max_angles_.end(), channel_count, 180.0); + } + } + } + } + + // 解析眼睛配置 + if (config_.has_eye()) { + const auto& eye = config_.eye(); + std::string serial_str = eye.serial(); + std::string offset_str = eye.offest(); + std::string min_angles_str = eye.jlmtlow(); + std::string max_angles_str = eye.jlmtup(); + + // 解析偏移角 + std::vector offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector min_angles; + if (!min_angles_str.empty()) { + std::istringstream min_ss(min_angles_str); + while (min_ss >> val) min_angles.push_back(val); + } + + std::vector max_angles; + if (!max_angles_str.empty()) { + std::istringstream max_ss(max_angles_str); + while (max_ss >> val) max_angles.push_back(val); + } + + // 解析 channel，如 64:4~9 + size_t colon = serial_str.find(':'); + if (colon != std::string::npos) { + uint8_t addr = std::stoi(serial_str.substr(0, colon)); + std::string range = serial_str.substr(colon + 1); + size_t tilde = range.find('~'); + if (tilde != std::string::npos) { + int ch_start = std::stoi(range.substr(0, tilde)); + int ch_end = std::stoi(range.substr(tilde + 1)); + int channel_count = ch_end - ch_start + 1; + + // 添加舵机通道 + for (int ch = ch_start; ch <= ch_end; ++ch) { + channels_.push_back({addr, static_cast(ch)}); + } + + // 添加角度限制 + if (min_angles.size() == 1) { + min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); + } else if (min_angles.size() >= channel_count) { + min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); + } else { + min_angles_.insert(min_angles_.end(), channel_count, 0.0); + } + + if (max_angles.size() == 1) { + max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); + } else if (max_angles.size() >= channel_count) { + max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); + } else { + max_angles_.insert(max_angles_.end(), channel_count, 180.0); + } + } + } + } + + // 解析嘴巴配置 + if (config_.has_mouth()) { + const auto& mouth = config_.mouth(); + std::string serial_str = mouth.serial(); + std::string offset_str = mouth.offest(); + std::string min_angles_str = mouth.jlmtlow(); + std::string max_angles_str = mouth.jlmtup(); + + // 解析偏移角 + std::vector offsets; + std::istringstream oss(offset_str); + double val; + while (oss >> val) offsets.push_back(val); + + // 解析角度限制 + std::vector min_angles; + if (!min_angles_str.empty()) { + std::istringstream min_ss(min_angles_str); + while (min_ss >> val) min_angles.push_back(val); + } + + std::vector max_angles; + if (!max_angles_str.empty()) { + std::istringstream max_ss(max_angles_str); + while (max_ss >> val) max_angles.push_back(val); + } + + // 解析 channel，如 65:0~9 + size_t colon = serial_str.find(':'); + if (colon != std::string::npos) { + uint8_t addr = std::stoi(serial_str.substr(0, colon)); + std::string range = serial_str.substr(colon + 1); + size_t tilde = range.find('~'); + if (tilde != std::string::npos) { + int ch_start = std::stoi(range.substr(0, tilde)); + int ch_end = std::stoi(range.substr(tilde + 1)); + int channel_count = ch_end - ch_start + 1; + + // 添加舵机通道 + for (int ch = ch_start; ch <= ch_end; ++ch) { + channels_.push_back({addr, static_cast(ch)}); + } + + // 添加角度限制 + if (min_angles.size() == 1) { + min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); + } else if (min_angles.size() >= channel_count) { + min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); + } else { + min_angles_.insert(min_angles_.end(), channel_count, 0.0); + } + + if (max_angles.size() == 1) { + max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); + } else if (max_angles.size() >= channel_count) { + max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); + } else { + max_angles_.insert(max_angles_.end(), channel_count, 180.0); + } + } + } + } + + // 确保所有向量大小一致 + if (min_angles_.size() != channels_.size()) { + min_angles_.resize(channels_.size(), 0.0); + } + + if (max_angles_.size() != channels_.size()) { + max_angles_.resize(channels_.size(), 180.0); + } + + current_joints_.resize(channels_.size(), 90.0); + last_joints_ = current_joints_; +} void BioHeadRobot::parseXmlConfig(const XmlNode &node) { diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp index 803ee75a..6fdaa099 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp @@ -21,7 +21,7 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) { return; } auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + auto &dmgr = DeviceManager::getInstance(); auto cam = dmgr.getDevice("cam4"); cam->start(); diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index ca0399d0..6fce7f5e 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -54,6 +54,7 @@ namespace cmvr::device { class AbstractCanbus : public AbstractDevice { public: + AbstractCanbus() = default; AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {} ~AbstractCanbus() {} /** diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index aef0af2c..28779a06 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -35,6 +35,17 @@ namespace cmvr { // LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what(); } + SocketCanClientRaw::SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config):config_(config) + { + auto channel_id = config_.channelid(); + port_ = static_cast(channel_id); + interface_ = CANCardParameter::NATIVE; + + enable_can_err_check_ = false; + + + } + void SocketCanClientRaw::init() { // if (!parameter.has_channel_id()) { diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 3de5b42c..04d57c86 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -26,6 +26,7 @@ #include "gflags/gflags.h" #include "../../abstract_canbus.h" #include "canbus/common/canbus_consts.h" +#include "cmvr/config/robot_config/robot_config.pb.h" namespace cmvr { namespace device { @@ -36,6 +37,7 @@ namespace cmvr { class SocketCanClientRaw final : public AbstractCanbus { public: explicit SocketCanClientRaw(const XmlNode &cfg); + SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config); ~SocketCanClientRaw(); void init() override; void start() override; @@ -73,6 +75,8 @@ namespace cmvr { // bool enable_can_err_check_{false}; + + cmvr::config::CanDeviceConfig config_; }; } } diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 532924d2..3b7669c9 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(ti5_motor) - +add_subdirectory(eyou_motor) # -------------------------------------------------------- # Unit test # -------------------------------------------------------- diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 710f88d2..bb89a415 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -41,6 +41,7 @@ namespace cmvr::device{ } JointStatus; public: + AbstractMotor() = default; explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {} AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {} ~AbstractMotor() override = default; diff --git a/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt b/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt new file mode 100644 index 00000000..abb62da2 --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt @@ -0,0 +1,19 @@ +add_library(eyoumotor SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/ethercat/eyou_motor_ethercat_protocol.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/eyou_motor.cpp + +) + + +target_include_directories(eyoumotor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +add_library(cmvr_es::device::eyoumotor ALIAS eyoumotor) + +target_link_libraries(eyoumotor + PRIVATE + eu_ethercat + protobuf + glog +) + +install(TARGETS eyoumotor LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp new file mode 100644 index 00000000..4e5edace --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp @@ -0,0 +1,652 @@ +// +// Created by cmvr on 2026/3/30. +// + +#include "eyou_motor_ethercat_protocol.h" +#include "canbus/canopen/register.h" +#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h" +#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h" +#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h" +#include "canbus/canopen/nmt_response_protocol.h" + +#include +#include +#include + +using namespace cmvr::device; +using namespace cmvr::msgs; + +// 构造函数 +EYouEthercatProtocol::EYouEthercatProtocol(std::string ifName) + : interface_name_(ifName), initialized_(false), slave_count_(0) { + comm_proto = CommProto::ETHERCAT; + + try { + // 初始化message_manager + message_manager_ = std::make_shared>(); + + // 初始化eu sdk + int ms = 1; + if (ETH_SUCCESS != eth_initDLL(interface_name_.c_str(), ms, &slave_count_)) { + LOG(ERROR) << "[EYouEthercatProtocol] eth_initDLL failed for interface: " << interface_name_; + eth_freeDLL(); + throw std::runtime_error("[EYouEthercatProtocol] eth_initDLL failed"); + } + + initialized_ = true; + LOG(INFO) << "[EYouEthercatProtocol] Initialized successfully, found " << slave_count_ << " slaves"; + + // 初始化从站状态 + for (int i = 0; i < slave_count_; i++) { + eth_State state; + if (eth_getSlaveState(i, &state) == ETH_SUCCESS) { + LOG(INFO) << "[EYouEthercatProtocol] Slave " << i << " state: " << state; + } + node_states_[i] = NodeState(); + } + + } catch (const std::exception& e) { + LOG(ERROR) << "[EYouEthercatProtocol] Constructor failed: " << e.what(); + throw std::runtime_error(e.what()); + } +} + +// 析构函数 +EYouEthercatProtocol::~EYouEthercatProtocol() { + // 失能所有电机 + // for (auto& node : node_states_) { + // if (node.second.enabled) { + // eth_disable(node.first); + // LOG(INFO) << "[EYouEthercatProtocol] Disabled motor " << node.first; + // } + // } + + if (initialized_) { + eth_freeDLL(); + LOG(INFO) << "[EYouEthercatProtocol] SDK freed successfully"; + } +} + +// 初始化节点 +bool EYouEthercatProtocol::initNode(uint8_t node_id) { + try { + int ret = 0; + if (node_id > slave_count_) { + LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " out of range (max: " << slave_count_ - 1 << ")"; + return false; + } + + // 初始化节点状态 + node_states_[node_id].initialized = true; + + // 设置初始操作模式为轮廓位置模式 + if (eth_setOperateMode(node_id, eth_OperateMode_ProfilePosition) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set operate mode for node " << node_id; + return false; + } + + // 等待模式切换完成 + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + // 使能电机 + ret = eth_enable(node_id); + if (ret != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable motor " << node_id << ",error: " << ret; + return false; + } + + node_states_[node_id].enabled = true; + node_states_[node_id].current_mode = RUN_MODE_PROFILE_POSITION; + + // 添加NMT响应协议 + message_manager_->AddRecvProtocolData, false>(node_id); + + LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " initialized successfully"; + return true; + + } catch (const std::exception& e) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to init node " << node_id << ": " << e.what(); + return false; + } +} + +// 设置运行模式 +void EYouEthercatProtocol::setMode(uint8_t node_id, RunMode mode) { + eth_OperateMode eth_mode; + + switch (mode) { + case RUN_MODE_PROFILE_POSITION: + eth_mode = eth_OperateMode_ProfilePosition; + break; + case RUN_MODE_VELOCITY: + eth_mode = eth_OperateMode_Velocity; + break; + case RUN_MODE_PROFILE_VELOCITY: + eth_mode = eth_OperateMode_ProfileVelocity; + break; + case RUN_MODE_TORQUE: + eth_mode = eth_OperateMode_ProfileTorque; + break; + case RUN_MODE_HOMING: + eth_mode = eth_OperateMode_Homing; + break; + case RUN_MODE_INTERPOLATED_POSITION: + eth_mode = eth_OperateMode_InterpolatedPosition; + break; + case RUN_MODE_CYCLIC_SYNC_POSITION: + eth_mode = eth_OperateMode_CyclicSyncPosition; + break; + case RUN_MODE_CYCLIC_SYNC_VELOCITY: + eth_mode = eth_OperateMode_CyclicSyncVelocity; + break; + case RUN_MODE_CYCLIC_SYNC_CURRENT: + eth_mode = eth_OperateMode_CyclicSyncTorque; + break; + default: + LOG(ERROR) << "[EYouEthercatProtocol] Unknown mode " << mode << " for node " << node_id; + return; + } + + if (eth_setOperateMode(node_id, eth_mode) == ETH_SUCCESS) { + node_states_[node_id].current_mode = mode; + LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " mode set to " << mode; + } else { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set mode for node " << node_id; + } +} + +// 设置目标位置和速度 +void EYouEthercatProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) { + auto pos_cmd = angleToPos(angle_rad); + auto speed = static_cast(std::abs(velToSpeed(vel))); + + msgs::RunMode current_mode = getMode(node_id); + + switch (current_mode) { + case RUN_MODE_PROFILE_POSITION: { + // 先设置轮廓速度 + eth_setProfileVelocity(node_id, speed); + // 设置目标位置 + eth_setTargetPosition(node_id, pos_cmd); + + // 构建控制字：使能状态 + 立即更新标志 + uint16_t ctlword = 0x0F | 0x20; // 0x0F使能, 0x20立即更新 + + // 触发位置更新 + eth_setControlWord(node_id, ctlword); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + // 启动新位置命令 + eth_setControlWord(node_id, ctlword | 0x10); + break; + } + case RUN_MODE_CYCLIC_SYNC_POSITION: + eth_setTargetPosition(node_id, pos_cmd); + break; + default: + LOG(WARNING) << "[EYouEthercatProtocol] setTarget(pos,vel) called in unsupported mode for node " << node_id; + break; + } +} + +// 设置目标速度 +void EYouEthercatProtocol::setTarget(uint8_t node_id, double vel) { + auto speed = static_cast(velToSpeed(vel)); + + msgs::RunMode current_mode = getMode(node_id); + + switch (current_mode) { + case RUN_MODE_VELOCITY: + case RUN_MODE_PROFILE_VELOCITY: + case RUN_MODE_CYCLIC_SYNC_VELOCITY: + eth_setTargetVelocity(node_id, speed); + break; + default: + LOG(WARNING) << "[EYouEthercatProtocol] setTarget(vel) called in unsupported mode for node " << node_id; + break; + } +} + +// 设置目标位置（修改的部分） +void EYouEthercatProtocol::setQ(uint8_t node_id, double angle_rad) { + auto pos_cmd = angleToPos(angle_rad); + + msgs::RunMode current_mode = getMode(node_id); + + switch (current_mode) { + case RUN_MODE_CYCLIC_SYNC_POSITION: + { + eth_setTargetPosition(node_id, pos_cmd); + break; + } + case RUN_MODE_PROFILE_POSITION: + { + // 1. 设置目标位置 + eth_setTargetPosition(node_id, pos_cmd); + + // 2. 构建控制字：使能状态 + 立即更新标志 + uint16_t ctlword = 0x0F | 0x20; // 0x0F使能, 0x20立即更新 + + // 3. 触发位置更新 + eth_setControlWord(node_id, ctlword); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + // 4. 启动新位置命令 + eth_setControlWord(node_id, ctlword | 0x10); + break; + } + default: + { + LOG(WARNING) << "[EYouEthercatProtocol] setQ called in unsupported mode for node " << node_id; + break; + } + } +} + +// 设置位置限制 +void EYouEthercatProtocol::setLimitQ(uint8_t node_id, double ub, double lb) { + int32_t lb_val = angleToPos(lb); + int32_t ub_val = angleToPos(ub); + // 开启软件位置限制 + hint32 limitEnable = 0x4c494d54; + if (eth_writeSDO(node_id, 0x2003, 0x00, &limitEnable, eth_DataType_int32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable software position limit for node " << node_id; + } + + // 设置软件位置限制 + if (eth_writeSDO(node_id, 0x607D, 0x01, &lb_val, eth_DataType_int32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set lower position limit for node " << node_id; + } + + if (eth_writeSDO(node_id, 0x607D, 0x02, &ub_val, eth_DataType_int32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set upper position limit for node " << node_id; + } +} + +// 设置速度限制 +void EYouEthercatProtocol::setLimitQd(uint8_t node_id, double qd) { + auto speed = static_cast(std::abs(velToSpeed(qd))); + + // 设置最大速度限制 + if (eth_writeSDO(node_id, 0x607F, 0x00, &speed, eth_DataType_uint32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set max speed limit for node " << node_id; + } + + if (eth_writeSDO(node_id, 0x6081, 0x00, &speed, eth_DataType_uint32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set profile speed for node " << node_id; + } +} + +// 设置加速度限制 +void EYouEthercatProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) { + // u_qdd: rad/s² (最大加速度) + // l_qdd: rad/s² (最大减速度) + // 转换为电机加速度（脉冲/秒²） + // 圈数/秒² = 弧度/秒² / 2π + // 脉冲/秒² = 圈数/秒² * GearRatio * PULSE_PER_REV + auto accel = static_cast((std::abs(u_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); + auto decel = static_cast((std::abs(l_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); + + // 通过SDO写入加速度和减速度限制 + if (eth_writeSDO(node_id, 0x6083, 0x00, &accel, eth_DataType_uint32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set acceleration limit for node " << node_id; + } + + if (eth_writeSDO(node_id, 0x6084, 0x00, &decel, eth_DataType_uint32, 100) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to set deceleration limit for node " << node_id; + } +} + +// 零位校准 +bool EYouEthercatProtocol::calibrateZeroQ(uint8_t node_id) { + try { + hint32 pos; + hint32 offset = 0; + hint32 rev = 0; + + //读取当前位置 + int iRet = eth_readSDO(node_id,0x6064,0x00,&pos,eth_DataType_uint32,20000); + if (iRet != ETH_SUCCESS) + { + std::cout <<"read actualPos last failed: "< temp || pos < -temp) + { + pos = pos % r; + if (pos > temp || pos < -temp) + { + if (pos > temp) + { + pos = pos -r; + } + else if(pos < -temp) + { + pos = pos + r; + } + } + std::cout <<"分辨率超过编码器范围，电机重新上电后变为零点" << std::endl; + + } + //// + + //写home offset偏移 + offset = -pos; + iRet = eth_writeSDO(node_id,0x607c,0x00,&offset,eth_DataType_uint32,20000); + if (iRet != ETH_SUCCESS) + { + std::cout <<"writeSdo offset -pos failed: "<(std::abs(velToSpeed(qd))); + + msgs::RunMode current_mode = getMode(node_id); + + switch (current_mode) { + case RUN_MODE_CYCLIC_SYNC_POSITION: + case RUN_MODE_PROFILE_POSITION: + eth_setProfileVelocity(node_id, speed); + last_Qd_[node_id] = speed; + break; + case RUN_MODE_PROFILE_VELOCITY: + case RUN_MODE_CYCLIC_SYNC_VELOCITY: + eth_setTargetVelocity(node_id, static_cast(speed)); + break; + default: + LOG(WARNING) << "[EYouEthercatProtocol] setQd called in unsupported mode for node " << node_id; + break; + } +} + +// 设置加速度 +void EYouEthercatProtocol::setQdd(uint8_t node_id, double qdd) { + // qdd: rad/s² (加速度) + // 转换为电机加速度（脉冲/秒²） + // 圈数/秒² = qdd / 2π + // 脉冲/秒² = 圈数/秒² * GearRatio * PULSE_PER_REV + uint32_t accel = static_cast((std::abs(qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); + + if (last_Qdd_[node_id] != accel) { + eth_setProfileAcceleration(node_id, accel); + eth_setProfileDeceleration(node_id, accel); + last_Qdd_[node_id] = accel; + } +} + +// 力矩关闭 +void EYouEthercatProtocol::torqueOff(uint8_t node_id) { + auto ret = eth_disable(node_id); + LOG(INFO) << "[EYouEthercatProtocol] Torque off for motor " << node_id << ", ret = " << ret; +} + +// 获取运行模式 +RunMode EYouEthercatProtocol::getMode(uint8_t node_id) { + eth_OperateMode mode; + if (eth_getOperateMode(node_id, &mode) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to read mode for node " << node_id; + return node_states_[node_id].current_mode; + } + + // 将SDK的操作模式转换为内部运行模式 + switch (mode) { + case eth_OperateMode_ProfilePosition: + return RUN_MODE_PROFILE_POSITION; + case eth_OperateMode_Velocity: + return RUN_MODE_VELOCITY; + case eth_OperateMode_ProfileVelocity: + return RUN_MODE_PROFILE_VELOCITY; + case eth_OperateMode_ProfileTorque: + return RUN_MODE_TORQUE; + case eth_OperateMode_Homing: + return RUN_MODE_HOMING; + case eth_OperateMode_InterpolatedPosition: + return RUN_MODE_INTERPOLATED_POSITION; + case eth_OperateMode_CyclicSyncPosition: + return RUN_MODE_CYCLIC_SYNC_POSITION; + case eth_OperateMode_CyclicSyncVelocity: + return RUN_MODE_CYCLIC_SYNC_VELOCITY; + case eth_OperateMode_CyclicSyncTorque: + return RUN_MODE_CYCLIC_SYNC_CURRENT; + default: + return RUN_MODE_UNSPECIFIED; + } +} + +// 配置轮廓参数 +void EYouEthercatProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { + eth_setProfileVelocity(node_id, speed); + eth_setProfileAcceleration(node_id, accel); + eth_setProfileDeceleration(node_id, decel); +} + +// 获取力矩 +double EYouEthercatProtocol::getTorque(uint8_t node_id) { + int16_t torque = 0; + if (eth_getActualTorque(node_id, &torque) != ETH_SUCCESS) { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to read torque for node " << node_id; + return 0.0; + } + return torque / 1000.0; // 转换为实际力矩值（额定力矩的倍数） +} + +// 获取状态字 +uint16_t EYouEthercatProtocol::getStatusWord(uint8_t node_id) { + uint16_t status_word = 0; + if (eth_getStatusWord(node_id, &status_word) == ETH_SUCCESS) { + node_states_[node_id].status_word = status_word; + } + return status_word; +} + +// 检查电机是否使能 +bool EYouEthercatProtocol::isEnabled(uint8_t node_id) { + uint16_t status_word = getStatusWord(node_id); + // 检查状态字的bit 2（操作使能）和bit 3（故障） + return (status_word & 0x000C) == 0x0008; // 使能状态且无故障 +} + +// 检查是否有故障 +bool EYouEthercatProtocol::isFault(uint8_t node_id) { + uint16_t status_word = getStatusWord(node_id); + return (status_word & 0x0008) != 0; // bit 3 为1表示故障 +} + +// 故障复位 +void EYouEthercatProtocol::faultReset(uint8_t node_id) { + if (isFault(node_id)) { + if (eth_faultReset(node_id) == ETH_SUCCESS) { + LOG(INFO) << "[EYouEthercatProtocol] Fault reset for motor " << node_id; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } else { + LOG(ERROR) << "[EYouEthercatProtocol] Failed to reset fault for motor " << node_id; + } + } +} + +// 等待目标到达 +bool EYouEthercatProtocol::waitForTargetReached(uint8_t node_id, int timeout_ms) { + return waitUntil([this, node_id]() { return reachedTargetQ(node_id); }, timeout_ms); +} + +// 获取RobotDetail数据 +std::unique_ptr EYouEthercatProtocol::GetRobotDetail() { + if (message_manager_) { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + return data_ptr; + } + return std::make_unique(); +} + +// 辅助函数实现（弧度制版本） +double EYouEthercatProtocol::posToAngle(int32_t pos) const { + // 脉冲数 -> 弧度 + // 圈数 = 脉冲数 / (GearRatio * PULSE_PER_REV) + // 弧度 = 圈数 * 2π + return (pos * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV); +} + +int32_t EYouEthercatProtocol::angleToPos(double angle) const { + // 弧度 -> 脉冲数 + // 圈数 = 弧度 / 2π + // 脉冲数 = 圈数 * GearRatio * PULSE_PER_REV + return static_cast((angle / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); +} + +double EYouEthercatProtocol::velToSpeed(double vel) const { + // vel: rad/s (角速度) + // 输出：电机速度（脉冲/秒） + // 圈数/秒 = vel / 2π + // 脉冲/秒 = 圈数/秒 * GearRatio * PULSE_PER_REV + return (vel / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV; +} + +double EYouEthercatProtocol::speedToVel(int32_t speed) const { + // speed: 电机速度（脉冲/秒） + // 输出：rad/s + // 圈数/秒 = speed / (GearRatio * PULSE_PER_REV) + // 弧度/秒 = 圈数/秒 * 2π + return (speed * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV); +} + +bool EYouEthercatProtocol::waitUntil(std::function condition, int timeout_ms) { + auto start = std::chrono::steady_clock::now(); + while (!condition()) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms)) { + return false; + } + } + return true; +} + +void EYouEthercatProtocol::updateNodeState(uint8_t node_id) { + // 更新状态字 + eth_getStatusWord(node_id, &node_states_[node_id].status_word); + // 可以根据需要更新其他状态信息 +} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h new file mode 100644 index 00000000..11401e97 --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h @@ -0,0 +1,106 @@ +// +// Created by cmvr on 2026/3/30. +// + +#pragma once +#include "cmvr/msgs/motor.pb.h" +#include "motor/motor_protocol_interface.h" +#include "../../../canbus/abstract_canbus.h" +#include "canbus/can_comm/can_receiver.h" +#include "canbus/can_comm/can_sender.h" +#include "canbus/can_comm/message_manager.h" +#include "cmvr/msgs/error_code.pb.h" +#include "cmvr/msgs/robot_detail.pb.h" + +#include "eyou_ethercat/include/eu_ethercat.h" + +#include +#include +#include +#include +#include +#include + +namespace cmvr { + namespace device { + + // 节点状态结构体 + struct NodeState { + bool enabled = false; // 是否使能 + bool initialized = false; // 是否已初始化 + msgs::RunMode current_mode = msgs::RUN_MODE_UNSPECIFIED; // 当前运行模式 + uint16_t status_word = 0; // 状态字缓存 + int32_t position_offset = 0; // 位置偏置值 + }; + + class EYouEthercatProtocol : public MotorProtocolInterface { + public: + explicit EYouEthercatProtocol(std::string ifName); + ~EYouEthercatProtocol() override; + + // 基础接口实现 + bool initNode(uint8_t node_id) override; + void setMode(uint8_t node_id, msgs::RunMode mode) override; + void setTarget(uint8_t node_id, double angle_rad, double vel) override; + void setTarget(uint8_t node_id, double vel) override; + void setQ(uint8_t node_id, double angle_rad) override; + void setLimitQ(uint8_t node_id, double ub, double lb) override; + void setLimitQd(uint8_t node_id, double qd) override; + void setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) override; + bool calibrateZeroQ(uint8_t node_id) override; + void brake(uint8_t node_id) override; + bool reachedTargetQ(uint8_t node_id) override; + + double getQ(uint8_t node_id) override; + double getQd(uint8_t node_id) override; + + void setQd(uint8_t node_id, double qd) override; + void setQdd(uint8_t node_id, double qdd) override; + + void torqueOff(uint8_t node_id) override; + msgs::RunMode getMode(uint8_t node_id) override; + + // 配置接口 + void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); + + // 扩展功能接口 + double getTorque(uint8_t node_id); // 获取力矩 + uint16_t getStatusWord(uint8_t node_id); // 获取状态字 + bool isEnabled(uint8_t node_id); // 检查电机是否使能 + bool isFault(uint8_t node_id); // 检查是否有故障 + void faultReset(uint8_t node_id); // 故障复位 + bool waitForTargetReached(uint8_t node_id, int timeout_ms); // 等待目标到达 + + // 数据获取接口 + std::unique_ptr GetRobotDetail(); + + private: + // 常量定义 + static constexpr double GearRatio = 101.0; // 电机减速比 + static constexpr double RADTODEG = 180.0 / M_PI; // 弧度转角度 + static constexpr double PULSE_PER_REV = 65536.0; // 每转脉冲数 + + // 辅助函数 + double posToAngle(int32_t pos) const; // 脉冲转角度 + int32_t angleToPos(double angle) const; // 角度转脉冲 + double velToSpeed(double vel) const; // 角速度转电机速度 + double speedToVel(int32_t speed) const; // 电机速度转角速度 + + bool waitUntil(std::function condition, int timeout_ms); + void updateNodeState(uint8_t node_id); // 更新节点状态 + + // 成员变量 + std::unordered_map node_states_; // 节点状态映射 + std::unordered_map last_Qd_; // 上次设置的速度 + std::unordered_map last_Qdd_; // 上次设置的加速度 + + std::shared_ptr> can_sender_; + std::shared_ptr> message_manager_; + + std::string interface_name_; // 网卡接口名 + bool initialized_; // 是否已初始化SDK + int slave_count_; // 从站数量 + }; + + } // namespace device +} // namespace cmvr \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp b/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp new file mode 100644 index 00000000..d9e2f043 --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp @@ -0,0 +1,5 @@ +// +// Created by cmvr on 2026/3/30. +// + +#include "eyou_motor.h" \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.h b/cmvr-es/devices/motor/eyou_motor/eyou_motor.h new file mode 100644 index 00000000..e94979e9 --- /dev/null +++ b/cmvr-es/devices/motor/eyou_motor/eyou_motor.h @@ -0,0 +1,51 @@ +// +// Created by cmvr on 2026/3/30. +// +#pragma once +#include "../abstract_motor.h" +#include "motor/motor_protocol_interface.h" +#include "motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h" + +namespace cmvr { + namespace device { + class EYouMotor : public AbstractMotor { + public: + EYouMotor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){} + EYouMotor(const XmlNode &config):AbstractMotor(config) { + info_.id = config.getAttrDefault("slave", -1); + info_.joint_name = config.getAttrString("jointName"); + info_.limitQ = config.getAttrDefault("limitQ", 3.14f); + info_.limitQd = config.getAttrDefault("limitQd", 0.5f); + node_id_ = info_.id; + } + + EYouMotor(const cmvr::config::EthercatMotorConfig& config) : AbstractMotor() { + info_.id = config.slave(); + info_.joint_name = config.jointname(); + info_.limitQ = config.limitqlb(); + info_.limitQd = config.limitqd(); + node_id_ = info_.id; + } + /** + * 电机初始化在具体的类中完成 + */ + void init() override { + if (!protocol_) { + throw std::runtime_error("Protocol not set for motor"); + } + if (protocol_->comm_proto == MotorProtocolInterface::CommProto::ETHERCAT ) { + auto canopen_protocol = std::dynamic_pointer_cast(protocol_); + // canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); + // canopen_protocol->setLimitQd(node_id_,5.0); + // canopen_protocol->setLimitQdd(node_id_,10.0,-10.0); + + // canopen_protocol->torqueOff(node_id_); + } + } + }; + + + } +} diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 634a715c..6b49aa06 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,7 +15,8 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - CUSTOM = 2 + ETHERCAT = 2, + CUSTOM = 3 }; virtual ~MotorProtocolInterface() = default; diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h index cfc9bde9..65f44ddc 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h @@ -1,11 +1,12 @@ // // Created by lgv on 2025/8/1. // +#pragma once #include "../abstract_motor.h" #include "motor/motor_protocol_interface.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" - +#include "cmvr/config/robot_config/robot_config.pb.h" /** * class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成 */ @@ -22,6 +23,15 @@ namespace cmvr { node_id_ = info_.id; } + Ti5Motor(const cmvr::config::MotorConfig& config) : AbstractMotor() { + info_.id = config.id(); + info_.joint_name = config.jointname(); + info_.limitQ = config.limitqlb(); + info_.limitQd = config.limitqd(); + node_id_ = info_.id; + } + + /** * 电机初始化在具体的类中完成 */ diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 5759c3af..91daf0ae 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -41,6 +41,7 @@ namespace cmvr::device{ class AbstractRobot: public AbstractDevice { public: + AbstractRobot() = default; explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {} ~AbstractRobot() override=default; diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 7770997b..9cd2d11d 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -35,6 +35,8 @@ #include "planner/joint_space_planner/include/joint_space_planner_creator.h" #include "planner/joint_space_planner/include/joint_space_planner.h" #include "ik_solver/include/pinocchio_dls_ik_solver.h" +#include "cmvr/config/robot_config/robot_config.pb.h" + namespace cmvr::device{ @@ -49,12 +51,10 @@ namespace cmvr::device{ ROBOT_ERROR } RobotStateMachine; -// typedef enum { -// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE -// } RobotRunningMode; - public: explicit HumanoidRobot(const XmlNode& cfg); + HumanoidRobot(const cmvr::config::HumanRobotConfig& config); + HumanoidRobot(const cmvr::config::EthercatRobotConfig& config); ~HumanoidRobot(); void init() override; @@ -232,6 +232,7 @@ namespace cmvr::device{ std::atomic is_head_busy_{false}, is_waist_busy_{false}; + std::string urdf_path_ = ""; }; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 0146c053..56c7542b 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -16,7 +16,7 @@ template HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { try { id_ = cfg.getAttrString("id"); - + urdf_path_ = cfg.getAttrString("urdf"); dof_ = DOF; if (!pathExists(cfg.getAttrString("urdf"))) { throw runtime_error("urdf file does not exist"); @@ -49,6 +49,87 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { } } + +template +HumanoidRobot::HumanoidRobot(const cmvr::config::HumanRobotConfig& config) { + try { + id_ = config.id(); + urdf_path_ = config.urdf(); + dof_ = DOF; + if (!pathExists(config.urdf())) { + throw runtime_error("urdf file does not exist"); + } + auto rcfg = cmvr::dyn::LoadRobotFromURDF( + config.urdf(), config.baselink()); + m_robot_ = std::make_shared >(rcfg); + + // 解析jointNames字符串，按逗号分隔 + joint_names_ = splitString(config.jointnames(), ","); + // 解析linkNames字符串，按逗号分隔 + link_names_ = splitString(config.linknames(), ","); + + if (joint_names_.size() != dof_) { + throw runtime_error("joint names size mismatched with dof"); + } + + m_state_ = m_robot_->MakeState(link_names_, joint_names_); + m_cctrl_ = make_shared >(m_robot_); + upd_freq_ = config.updfreq(); + CSP_buffer_ = make_shared >(config.buffersize()); + CSV_buffer_ = make_shared >(config.buffersize()); + CSC_buffer_ = make_shared >(config.buffersize()); + + upd_freq_ = 1000; + upd_timer_ = make_shared(); + upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), + [this] { update_state_(); }); + rsm_.store(ROBOT_READY); + } catch (exception &e) { + LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; + throw runtime_error(e.what()); + } +} + +template +HumanoidRobot::HumanoidRobot(const cmvr::config::EthercatRobotConfig& config) { + try { + id_ = config.id(); + urdf_path_ = config.urdf(); + dof_ = DOF; + if (!pathExists(config.urdf())) { + throw runtime_error("urdf file does not exist"); + } + auto rcfg = cmvr::dyn::LoadRobotFromURDF( + config.urdf(), config.baselink()); + m_robot_ = std::make_shared >(rcfg); + + // 解析jointNames字符串，按逗号分隔 + joint_names_ = splitString(config.jointnames(), ","); + // 解析linkNames字符串，按逗号分隔 + link_names_ = splitString(config.linknames(), ","); + + if (joint_names_.size() != dof_) { + throw runtime_error("joint names size mismatched with dof"); + } + + m_state_ = m_robot_->MakeState(link_names_, joint_names_); + m_cctrl_ = make_shared >(m_robot_); + upd_freq_ = config.updfreq(); + CSP_buffer_ = make_shared >(config.buffersize()); + CSV_buffer_ = make_shared >(config.buffersize()); + CSC_buffer_ = make_shared >(config.buffersize()); + + upd_freq_ = 1000; + upd_timer_ = make_shared(); + upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), + [this] { update_state_(); }); + rsm_.store(ROBOT_READY); + } catch (exception &e) { + LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; + throw runtime_error(e.what()); + } +} + template void HumanoidRobot::init() { motor_manager_ = MotorsInfo::getInstance()->getMotorManager(); @@ -59,7 +140,7 @@ void HumanoidRobot::init() { std::vector(7, 1.0)); - ik_solver_ = std::make_shared("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + ik_solver_ = std::make_shared(urdf_path_, "PELVIS_S", "R_WRIST_R_S", "R_FINGER_TIP_FIXED"); diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index d1b47e4e..1fba8212 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -112,7 +112,7 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(); auto robot_abs = dmgr.getDevice("hc01"); auto robot = std::dynamic_pointer_cast>(robot_abs); @@ -231,7 +231,7 @@ TEST(HumanoidRobotTest, moveLSCurveLocalRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(); auto robot_abs = dmgr.getDevice("hc01"); auto robot = std::dynamic_pointer_cast>(robot_abs); @@ -368,7 +368,7 @@ TEST(HumanoidRobotTest, moveLRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(); auto robot_abs = dmgr.getDevice("hc01"); auto robot = std::dynamic_pointer_cast>(robot_abs); @@ -469,7 +469,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(); auto robot = dmgr.getDevice("hc01"); auto camera = dmgr.getDevice("right_hand_cam"); @@ -691,7 +691,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + auto& dmgr = DeviceManager::getInstance(); auto robot = dmgr.getDevice("hc01"); auto camera = dmgr.getDevice("right_hand_cam"); @@ -990,3 +990,21 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps."; } + + +TEST(HumanoidRobotTest, EYouTest) { + auto& dmgr = DeviceManager::getInstance(); + auto robot_abs = dmgr.getDevice("ec_robot_01"); + auto robot = std::dynamic_pointer_cast>(robot_abs); + ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; + + std::vector init_cmd = { + {"L_SHOULDER_P", 0.1} + }; + // robot->moveJ(init_cmd, 0.1, 1.0); + + // robot->calibrateZeroQ("L_SHOULDER_P"); + + robot->torqueOff("L_SHOULDER_P"); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); +} diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index ab0f76d3..dd88b4d3 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -9,27 +9,13 @@ #include #include "service/grpc/include/server_runner.h" #include "utils/base/include/logger.h" +#include "common/utils/config_helper/include/config_helper.h" using namespace cmvr::service; int main(int argc, char* argv[]) { - std::string config_path; - if(argc == 1) { - char exe_path[PATH_MAX] = {0}; - ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX); - if (count == -1) { - std::cerr << "Failed to read /proc/self/exe" << std::endl; - return 1; - } - std::string exe_dir = dirname(exe_path); - config_path = exe_dir + "/config/cabin_robot.xml"; - std::cout << "Using default config path: " << config_path << std::endl; - } - else if (argc == 2){ - config_path = argv[1]; - } - - ServerRunner runner; - const XmlNode config(config_path); - runner.start(config); + cmvr::config::ServerConfig server_config; + cmvr::ConfigHelper::getServerConfig(server_config); + ServerRunner runner(server_config); + runner.start(); runner.join(); google::ShutdownGoogleLogging(); return 0; diff --git a/cmvr-es/monitor/abstract_monitor.h b/cmvr-es/monitor/abstract_monitor.h index 6b707e9c..244e5722 100644 --- a/cmvr-es/monitor/abstract_monitor.h +++ b/cmvr-es/monitor/abstract_monitor.h @@ -13,6 +13,7 @@ namespace cmvr::monitor{ class AbstractMonitor { public: + AbstractMonitor() = default; explicit AbstractMonitor(const XmlNode &cfg){ cfg_ = cfg; freq_ = cfg_.getAttrDefault("freq", 1); diff --git a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h index 89664362..a22256b3 100644 --- a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h +++ b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h @@ -8,17 +8,21 @@ #include #include "../../abstract_monitor.h" +#include "cmvr/config/monitor_config/monitor_config.pb.h" namespace cmvr::monitor { class DiskMonitor final : public AbstractMonitor{ public: explicit DiskMonitor(const XmlNode& config); + explicit DiskMonitor(const cmvr::config::DiskMonitorConfig& config); // 新增proto构造函数 ~DiskMonitor(); protected: bool check_() override; bool execution_() override; private: + void initFromProto(const cmvr::config::DiskMonitorConfig& config); // 新增初始化方法 + std::unordered_map folders_; std::vector dirsToClean_; // 需要清理的目录列表 mutable std::mutex mutex_; // 保护共享资源的互斥锁 @@ -27,6 +31,4 @@ namespace cmvr::monitor { }; } - - #endif //FOLDER_MONITOR_H \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp index 20630615..9f4497cf 100644 --- a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp +++ b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp @@ -14,7 +14,6 @@ using namespace cmvr::monitor; namespace fs = std::filesystem; using namespace std::chrono; - // 文件信息结构体 struct FileInfo { std::string path; @@ -38,51 +37,40 @@ bool deleteOldestFile(const std::string& directoryPath) { struct dirent* entry; struct stat fileStat; - // 遍历目录中的每个条目 while ((entry = readdir(dir)) != nullptr) { - // 跳过 "." 和 ".." if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") { continue; } - // 构建完整路径 std::string filePath = directoryPath + "/" + entry->d_name; - // 获取文件状态信息 if (stat(filePath.c_str(), &fileStat) == -1) { std::cerr << "Failed to get file status: " << filePath << std::endl; continue; } - // 只处理普通文件 if (S_ISREG(fileStat.st_mode)) { FileInfo fileInfo; fileInfo.path = filePath; - - // 优先使用 st_birthtime (如果可用)，否则使用 st_ctime #ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME fileInfo.creationTime = fileStat.st_birthtime; #else fileInfo.creationTime = fileStat.st_ctime; #endif - files.push_back(fileInfo); } } closedir(dir); - // 如果没有找到文件，返回失败 if (files.empty()) { std::cout << "No files found in directory: " << directoryPath << std::endl; return false; } - // 按创建时间排序 std::sort(files.begin(), files.end(), compareByCreationTime); - - // 删除最早创建的文件 std::string oldestFilePath = files[0].path; + if (unlink(oldestFilePath.c_str()) == -1) { std::cerr << "Failed to delete file: " << oldestFilePath << std::endl; return false; @@ -92,11 +80,11 @@ bool deleteOldestFile(const std::string& directoryPath) { return true; } - bool pathExists(const std::string& path) { struct stat buffer; return (stat(path.c_str(), &buffer) == 0); } + int countFilesInDirectory(const std::string& path) { DIR* dir = opendir(path.c_str()); if (!dir) { @@ -107,9 +95,7 @@ int countFilesInDirectory(const std::string& path) { int count = 0; struct dirent* entry; - // 遍历目录中的每个条目 while ((entry = readdir(dir)) != nullptr) { - // 跳过 "." 和 ".." if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") { count++; } @@ -119,8 +105,8 @@ int countFilesInDirectory(const std::string& path) { return count; } +// XML构造函数 DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ - try { for (auto &node: config.getChildren()){ string fileDir = node.getAttrString("fileDir"); @@ -138,13 +124,42 @@ DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ throw runtime_error("[DiskMonitor] (DiskMonitor): Failed "+ string(e.what())); } } + +// Proto构造函数 +DiskMonitor::DiskMonitor(const cmvr::config::DiskMonitorConfig& config) : AbstractMonitor() { + try { + initFromProto(config); + is_initialized_ = true; + LOG(INFO) << "[DiskMonitor]: Init DiskMonitor from proto Success"; + } + catch (const exception& e) { + LOG(ERROR) << "[DiskMonitor] (DiskMonitor): Failed " << e.what(); + throw runtime_error("[DiskMonitor] (DiskMonitor): Failed " + string(e.what())); + } +} + +// Proto初始化方法 +void DiskMonitor::initFromProto(const cmvr::config::DiskMonitorConfig& config) { + // 设置监控ID和频率（通过AbstractMonitor） + // 假设AbstractMonitor有setId和setFreq方法，如果没有需要添加 + // 这里根据你的AbstractMonitor实现来调整 + + for (const auto& folder : config.folders()) { + string fileDir = folder.filedir(); + if (!pathExists(fileDir)) { + throw runtime_error("[DiskMonitor] (initFromProto): path not exists: " + fileDir); + } + int maxVolume = folder.maxvolume(); + folders_[fileDir] = maxVolume; + } +} + DiskMonitor::~DiskMonitor() { folders_.clear(); } + bool DiskMonitor::check_() { - // 检查当前目录下的文件数量是否超过最大值 bool allOk = true; - // 遍历所有监控的文件夹 for (const auto& folder : folders_) { const std::string& path = folder.first; size_t maxFiles = folder.second; @@ -154,28 +169,21 @@ bool DiskMonitor::check_() { continue; } int nFileCount = countFilesInDirectory(path); - if (nFileCount > maxFiles) - { + if (nFileCount > maxFiles) { allOk = false; dirsToClean_.push_back(path); std::cout << "path need remove oldest file: " << path << std::endl; } - - } catch (const fs::filesystem_error& e) { std::cerr << "Filesystem error: " << e.what() << '\n'; } catch (const std::exception& e) { std::cerr << "Unexpected error: " << e.what() << '\n'; } } - return allOk; } - bool DiskMonitor::execution_() { - - // 删除创建时间最早的文件 std::lock_guard lock(mutex_); bool success = true; @@ -183,6 +191,6 @@ bool DiskMonitor::execution_() { deleteOldestFile(path); } - dirsToClean_.clear(); // 清理完成后清空列表 + dirsToClean_.clear(); return success; } \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_factory.h b/cmvr-es/monitor_manager/include/monitor_factory.h index a0dbc900..cdd879a3 100644 --- a/cmvr-es/monitor_manager/include/monitor_factory.h +++ b/cmvr-es/monitor_manager/include/monitor_factory.h @@ -5,8 +5,8 @@ #ifndef MONITOR_FACTORY_H #define MONITOR_FACTORY_H - -#include "../../monitor/abstract_monitor.h" +#include "monitor/abstract_monitor.h" +#include "cmvr/config/monitor_config/monitor_config.pb.h" namespace cmvr::monitor { @@ -14,14 +14,17 @@ namespace cmvr::monitor { public: MonitorFactory() = default; + + // 原有的XML方式 template std::shared_ptr create(const XmlNode& cfg); std::shared_ptr create(const XmlNode& cfg); + + // 新增proto方式 + template + std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); + std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); }; - - } - - #endif //MONITOR_FACTORY_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_manager.h b/cmvr-es/monitor_manager/include/monitor_manager.h index 8c53a401..422ece1f 100644 --- a/cmvr-es/monitor_manager/include/monitor_manager.h +++ b/cmvr-es/monitor_manager/include/monitor_manager.h @@ -15,23 +15,38 @@ namespace cmvr::monitor { public: MonitorManager(const MonitorManager&) = delete; MonitorManager& operator=(const MonitorManager&) = delete; + + // 原有的XML方式 static MonitorManager& getInstance(const XmlNode &cfg); + + // 新增proto方式 + static MonitorManager& getInstance(const cmvr::config::MonitorManagerConfig& config); + static MonitorManager& getInstance(); static void destroyInstance(); private: - explicit MonitorManager (const XmlNode &cfg); - void init_monitors(); + // 原有的XML方式 + explicit MonitorManager(const XmlNode &cfg); + + // 新增proto方式 + explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config); + + void init_monitors(); // XML方式 + void init_monitors_from_proto(); // Proto方式 + private: static std::once_flag init_flag_; static std::shared_ptr instance_; - XmlNode cfg_; + // 两种配置方式，只有一个有效 + bool use_proto_ = false; + XmlNode cfg_; // XML配置 + cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置 + std::shared_ptr monitor_factory_; std::unordered_map> monitors_; }; } - - #endif //MONITOR_MANAGER_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_factory.cpp b/cmvr-es/monitor_manager/src/monitor_factory.cpp index 821373e2..e770f294 100644 --- a/cmvr-es/monitor_manager/src/monitor_factory.cpp +++ b/cmvr-es/monitor_manager/src/monitor_factory.cpp @@ -3,12 +3,12 @@ // #include "../include/monitor_factory.h" - #include "../../monitor/diskmonitor/include/disk_monitor.h" using namespace std; using namespace cmvr::monitor; +// XML方式的模板特化 template std::shared_ptr MonitorFactory::create(const XmlNode&); template @@ -17,8 +17,30 @@ std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { return std::make_shared(cfg); } else { - LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; - throw runtime_error("[DeviceFactory]: Unsupported device type"); + LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; + throw runtime_error("[MonitorFactory]: Unsupported monitor type"); } } +// Proto方式的模板特化 +template std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig&); + +template +std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { + if constexpr (std::is_same_v) { + return std::make_shared(config); + } + else { + LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; + throw runtime_error("[MonitorFactory]: Unsupported monitor type"); + } +} + +std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { + // 保留原有实现 + return nullptr; +} + +std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { + return std::make_shared(config); +} \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_manager.cpp b/cmvr-es/monitor_manager/src/monitor_manager.cpp index 2dd9e2cf..dde9b386 100644 --- a/cmvr-es/monitor_manager/src/monitor_manager.cpp +++ b/cmvr-es/monitor_manager/src/monitor_manager.cpp @@ -3,8 +3,7 @@ // #include "../include/monitor_manager.h" - -#include "../../monitor/diskmonitor/include/disk_monitor.h" +#include "monitor/diskmonitor/include/disk_monitor.h" using namespace std; using namespace cmvr::monitor; @@ -12,11 +11,19 @@ using namespace cmvr::monitor; std::shared_ptr MonitorManager::instance_ = nullptr; std::once_flag MonitorManager::init_flag_; -MonitorManager::MonitorManager(const XmlNode& cfg) { +// XML方式构造函数 +MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) { monitor_factory_ = make_shared(); - cfg_ = cfg; init_monitors(); } + +// Proto方式构造函数 +MonitorManager::MonitorManager(const cmvr::config::MonitorManagerConfig& config) : use_proto_(true), proto_cfg_(config) { + monitor_factory_ = make_shared(); + init_monitors_from_proto(); +} + +// XML方式单例 MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { std::call_once(init_flag_, [&cfg] { instance_.reset(new MonitorManager(cfg)); @@ -24,9 +31,17 @@ MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { return *instance_; } +// Proto方式单例 +MonitorManager& MonitorManager::getInstance(const cmvr::config::MonitorManagerConfig& config) { + std::call_once(init_flag_, [&config] { + instance_.reset(new MonitorManager(config)); + }); + return *instance_; +} + MonitorManager& MonitorManager::getInstance() { if (!instance_) { - throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first."); + throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance() first."); } return *instance_; } @@ -35,13 +50,14 @@ void MonitorManager::destroyInstance() { instance_.reset(); } +// XML方式初始化 void MonitorManager::init_monitors() { - try{ + try { auto diskmonitor_node = cfg_.getChild("DiskMonitor"); auto id = diskmonitor_node.getAttrString("id"); auto monitor = monitor_factory_->create(diskmonitor_node); if (monitor == nullptr) { - throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); + throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id); } monitors_[id] = monitor; } @@ -49,5 +65,31 @@ void MonitorManager::init_monitors() { LOG(ERROR) << e.what(); throw runtime_error(e.what()); } +} +// Proto方式初始化 +void MonitorManager::init_monitors_from_proto() { + try { + if (!proto_cfg_.has_disk_monitor()) { + LOG(WARNING) << "[MonitorManager]: No disk_monitor config found"; + return; + } + + const auto& disk_monitor_cfg = proto_cfg_.disk_monitor(); + if (!disk_monitor_cfg.enable()) { + LOG(INFO) << "[MonitorManager]: Disk monitor is disabled"; + return; + } + + auto id = disk_monitor_cfg.id(); + auto monitor = monitor_factory_->create(disk_monitor_cfg); + if (monitor == nullptr) { + throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id); + } + monitors_[id] = monitor; + } + catch (const exception& e) { + LOG(ERROR) << e.what(); + throw runtime_error(e.what()); + } } \ No newline at end of file diff --git a/cmvr-es/service/grpc/include/server_runner.h b/cmvr-es/service/grpc/include/server_runner.h index be8eadf8..e148c811 100644 --- a/cmvr-es/service/grpc/include/server_runner.h +++ b/cmvr-es/service/grpc/include/server_runner.h @@ -11,13 +11,14 @@ #include #include #include "rapidxml/xml_parser.h" +#include "cmvr/config/server_config/server_config.pb.h" namespace cmvr::service { class ServerRunner { public: ServerRunner(); - explicit ServerRunner(XmlNode cfg); + ServerRunner(const cmvr::config::ServerConfig& config); ~ServerRunner(); ServerRunner(const ServerRunner&) = delete; @@ -25,7 +26,7 @@ namespace cmvr::service { ServerRunner(ServerRunner&&) = delete; ServerRunner& operator=(ServerRunner&&) = delete; - void start(XmlNode cfg); + void start(); void stop(); void join(); @@ -40,7 +41,7 @@ namespace cmvr::service { std::condition_variable cv_; std::thread worker_; - XmlNode cfg_; + cmvr::config::ServerConfig cfg_; std::unique_ptr server_; std::string address_; diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index 487fe705..0c4e8469 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -17,7 +17,7 @@ using google::protobuf::util::TimeUtil; gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()) { - pinocchio_ik_ = std::make_shared("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + pinocchio_ik_ = std::make_shared("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" ,"PELVIS_S" ,"R_WRIST_R_S"); pinocchio_ik_->init(); diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp index dc689aff..ec04091a 100644 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -25,23 +25,21 @@ using namespace cmvr::service; ServerRunner::ServerRunner() = default; -ServerRunner::ServerRunner(XmlNode cfg) { - start(std::move(cfg)); +ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config) +{ + // start(); } - ServerRunner::~ServerRunner() { stop(); join(); } -void ServerRunner::start(XmlNode cfg) { +void ServerRunner::start() { std::unique_lock lk(mtx_); if (worker_.joinable()) { throw std::runtime_error("ServerRunner already started"); } - - cfg_ = std::move(cfg); stop_requested_ = false; started_ = false; start_failed_ = false; @@ -97,25 +95,15 @@ void ServerRunner::threadMain() { using namespace cmvr::monitor; try { - if (!cfg_.hasChild("DeviceManager")) { - throw std::runtime_error("Device Manager node not found"); - } + static std::once_flag reflection_once; std::call_once(reflection_once, []() { grpc::reflection::InitProtoReflectionServerBuilderPlugin(); }); + DeviceManager::getInstance(); - auto dmgr_cfg = cfg_.getChild("DeviceManager"); - DeviceManager::getInstance(dmgr_cfg); - - if (cfg_.hasChild("MonitorManager")) { - auto mmgr_cfg = cfg_.getChild("MonitorManager"); - MonitorManager::getInstance(mmgr_cfg); - } - - auto grpc_cfg = cfg_.getChild("gRPCServer"); - std::string port = grpc_cfg.getAttrDefault("port", "50051"); + std::string port = cfg_.grpc_server().port(); std::string local_address = "0.0.0.0:" + port; auto camera_service = std::make_unique(); diff --git a/protos/cmvr/config/biohead_config/biohead_config.proto b/protos/cmvr/config/biohead_config/biohead_config.proto new file mode 100644 index 00000000..80c7d052 --- /dev/null +++ b/protos/cmvr/config/biohead_config/biohead_config.proto @@ -0,0 +1,44 @@ +// biohead.proto +syntax = "proto3"; + +package cmvr.config; + +// 眉毛配置 +message EyeBrowConfig { + string serial = 1; // 如 "64:0~3" + string offest = 2; // 偏移量，如 "90 90 90 90" + string jLmtUp = 3; // 上限，如 "90 170 155 110" + string jLmtLow = 4; // 下限，如 "20 77 90 20" +} + +// 眼睛配置 +message EyeConfig { + string serial = 1; // 如 "64:4~9" + string offest = 2; // 偏移量，如 "90 90 90 90 90 90" + string jLmtUp = 3; // 上限，如 "90 150 165 90 120 115" + string jLmtLow = 4; // 下限，如 "20 90 90 25 70 75" +} + +// 嘴巴配置 +message MouthConfig { + string serial = 1; // 如 "65:0~9" + string offest = 2; // 偏移量，如 "90 90 90 90 90 90 90 90 90 90" + string jLmtUp = 3; // 上限，如 "150 110 130 140 100 105 110 125 90 95" + string jLmtLow = 4; // 下限，如 "70 30 80 80 65 55 45 80 85 90" +} + +// BioHead配置 +message BioHeadConfig { + string id = 1; // 设备ID，如 "bio_head" + string serial = 2; // 串口路径，如 "/dev/ttyUSB0" + int32 ctrlFreq = 3; // 控制频率，如 50 + + EyeBrowConfig eyebrow = 4; // 眉毛配置 + EyeConfig eye = 5; // 眼睛配置 + MouthConfig mouth = 6; // 嘴巴配置 + bool enable = 7; // 是否启用 +} + +message HeadConfig { + repeated BioHeadConfig bioheads = 3; +} \ No newline at end of file diff --git a/protos/cmvr/config/monitor_config/monitor_config.proto b/protos/cmvr/config/monitor_config/monitor_config.proto new file mode 100644 index 00000000..368c4660 --- /dev/null +++ b/protos/cmvr/config/monitor_config/monitor_config.proto @@ -0,0 +1,22 @@ +syntax = "proto3"; + +package cmvr.config; + +// 文件夹监控配置 +message FolderConfig { + string fileDir = 1; // 文件夹路径 + int32 maxVolume = 2; // 最大容量（MB） +} + +// 磁盘监控配置 +message DiskMonitorConfig { + string id = 1; // 监控器ID，如 "file_monitor" + int32 freq = 2; // 监控频率（Hz） + repeated FolderConfig folders = 3; // 要监控的文件夹列表 + bool enable = 4; // 是否启用 +} + +// 监控管理器配置 +message MonitorManagerConfig { + DiskMonitorConfig disk_monitor = 1; // 磁盘监控配置 +} \ No newline at end of file diff --git a/protos/cmvr/config/robot_config/robot_config.proto b/protos/cmvr/config/robot_config/robot_config.proto new file mode 100644 index 00000000..1838f1eb --- /dev/null +++ b/protos/cmvr/config/robot_config/robot_config.proto @@ -0,0 +1,88 @@ +syntax = "proto3"; + +package cmvr.config; + +// 电机配置 +message MotorConfig { + int32 id = 1; // 电机ID + string jointName = 2; // 关节名称 + float limitQLb = 3; // 位置下限 + float limitQUb = 4; // 位置上限 + float limitQd = 5; // 速度限制 +} + +// 通用的CAN设备配置（手臂、头部、腰部都可以复用） +message CanDeviceConfig { + string id = 1; // CAN ID + string devId = 2; // 设备ID + int32 channelId = 3; // 通道ID + bool enable = 4; // 是否启用 + string toolFrame = 5; // 工具坐标系（手臂需要，头/腰可选） + repeated MotorConfig motors = 6; // 电机列表 +} + +// CAN管理器配置 +message CanManagerConfig { + string id = 1; // CAN管理器ID + string devId = 2; // 设备ID + CanDeviceConfig left_arm = 3; // 左臂CAN + CanDeviceConfig right_arm = 4; // 右臂CAN + CanDeviceConfig head = 5; // 头部CAN + CanDeviceConfig waist = 6; // 腰部CAN +} + +// 人形机器人配置 +message HumanRobotConfig { + string id = 1; // 机器人ID，如 "hc01" + int32 dof = 2; // 自由度，如 14 + string urdf = 3; // URDF文件路径 + string baseLink = 4; // 基座link名称，如 "PELVIS_S" + string jointNames = 5; // 关节名称列表，逗号分隔 + string linkNames = 6; // link名称列表，逗号分隔 + int32 bufferSize = 7; // 缓冲区大小，如 50 + bool verbose = 8; // 是否详细输出 + int32 updFreq = 9; // 更新频率，如 500 + CanManagerConfig can_manager = 10; // CAN管理器配置 + bool enable = 11; // 是否启用 +} + +// EtherCAT主站配置 +message EthercatMasterConfig { + string interface = 1; // 网络接口，如 "eth0" + int32 cycle_time_us = 2; // 周期时间（微秒），默认1000 + bool enable = 3; // 是否启用 +} + +// EtherCAT电机驱动配置 +message EthercatMotorConfig { + int32 slave = 1; // 从站ID + string jointName = 2; // 关节名称 + float limitQLb = 3; // 位置下限 + float limitQUb = 4; // 位置上限 + float limitQd = 5; // 速度限制 +} + +// EtherCAT机器人配置 +message EthercatRobotConfig { + string id = 1; // 机器人ID + int32 dof = 2; // 自由度 + string urdf = 3; // URDF文件路径 + string baseLink = 4; // 基座link名称 + string jointNames = 5; // 关节名称列表，逗号分隔 + string linkNames = 6; // link名称列表，逗号分隔 + int32 bufferSize = 7; // 缓冲区大小 + bool verbose = 8; // 是否详细输出 + int32 updFreq = 9; // 更新频率（Hz） + bool enable = 10; // 是否启用 + + EthercatMasterConfig master = 11; // EtherCAT主站配置 + repeated EthercatMotorConfig motors = 12; // 电机配置列表 +} + + + +// 主配置 +message RobotConfig { + repeated HumanRobotConfig humanrobots = 1; + repeated EthercatRobotConfig ethercat_robots = 2; // 新增的EtherCAT机器人 +} \ No newline at end of file diff --git a/protos/cmvr/config/server_config/server_config.proto b/protos/cmvr/config/server_config/server_config.proto new file mode 100644 index 00000000..911b12ca --- /dev/null +++ b/protos/cmvr/config/server_config/server_config.proto @@ -0,0 +1,10 @@ +syntax = "proto3"; +package cmvr.config; + +message GrpcServerConfig{ + string port = 1; +} + +message ServerConfig{ + GrpcServerConfig grpc_server = 1; +} \ No newline at end of file diff --git a/request.txt b/request.txt index 9a4d60e2..cc0986ba 100644 --- a/request.txt +++ b/request.txt @@ -33,5 +33,6 @@ third_party/modbus/3.1.11 third_party/visp/3.7.0 third_party/mainif/0.0.5 third_party/matplotplusplus/1.2.0 +third_party/eyou_ethercat