From fb304df94c869b076a81c4f382ad4e995eae51e0 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 2 Jun 2026 11:31:42 +0800 Subject: [PATCH] Revert "update proto config" This reverts commit 9fb185b5a89d88f77a09fbe2ef041ed1ef22b30c. --- .../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 | 143 ++-- 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, 580 insertions(+), 2022 deletions(-) delete mode 100644 cmvr-es/common/config/biohead_config/biohead_config.pb.txt delete mode 100644 cmvr-es/common/config/monitor_config/monitor_config.pb.txt delete mode 100644 cmvr-es/common/config/robot_config/robot_config.pb.txt delete mode 100644 cmvr-es/common/config/server_config/server_config.pb.txt delete mode 100644 cmvr-es/devices/motor/eyou_motor/CMakeLists.txt delete mode 100644 cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp delete mode 100644 cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h delete mode 100644 cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp delete mode 100644 cmvr-es/devices/motor/eyou_motor/eyou_motor.h delete mode 100644 protos/cmvr/config/biohead_config/biohead_config.proto delete mode 100644 protos/cmvr/config/monitor_config/monitor_config.proto delete mode 100644 protos/cmvr/config/robot_config/robot_config.proto delete 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 08aa560e..b2c178fe 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(); + cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager")); // 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 deleted file mode 100644 index b2880e64..00000000 --- a/cmvr-es/common/config/biohead_config/biohead_config.pb.txt +++ /dev/null @@ -1,27 +0,0 @@ -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 e69de29b..d67dffac 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -0,0 +1,152 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ 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 f35f4018..18b02c30 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: false + enable: true } 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 6adf262b..e6783dd2 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.223" + ip: "192.168.1.213" 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: false + enable: true } 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 deleted file mode 100644 index d5350f3c..00000000 --- a/cmvr-es/common/config/monitor_config/monitor_config.pb.txt +++ /dev/null @@ -1,25 +0,0 @@ -# 监控管理器配置 -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 deleted file mode 100644 index cfbd4cd5..00000000 --- a/cmvr-es/common/config/robot_config/robot_config.pb.txt +++ /dev/null @@ -1,212 +0,0 @@ -# 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 deleted file mode 100644 index 78599a06..00000000 --- a/cmvr-es/common/config/server_config/server_config.pb.txt +++ /dev/null @@ -1,3 +0,0 @@ -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 445af379..7d189128 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/cmvr-es/common/utils/config_helper/include/config_helper.h @@ -10,9 +10,6 @@ #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 { \ @@ -85,20 +82,6 @@ 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 caedf533..5d58fb9e 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/cmvr-es/common/utils/config_helper/include/config_setting.h @@ -6,6 +6,3 @@ 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 528819f5..9e44092d 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -88,15 +88,3 @@ 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 ff66818d..d0fda580 100644 --- a/cmvr-es/data_center/CMakeLists.txt +++ b/cmvr-es/data_center/CMakeLists.txt @@ -11,7 +11,6 @@ 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 4c86ce00..56c2de04 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 cmvr::config::HumanRobotConfig& config); - void init(const cmvr::config::EthercatRobotConfig& config); + + void init(const XmlNode &cfg); 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 c1eafea6..4b6a087c 100644 --- a/cmvr-es/data_center/src/motors_info.cpp +++ b/cmvr-es/data_center/src/motors_info.cpp @@ -3,8 +3,6 @@ // #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" @@ -14,95 +12,99 @@ using namespace cmvr::device; MotorsInfo::MotorsInfo() { } -void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) { - const auto& can_manager = config.can_manager(); +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"); - // 读取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(); + auto l_can_cfg = can_cfg.getChild("LeftArmCan"); + left_arm_enabled_ = readEnable(l_can_cfg); - // 局部变量存储电机配置 - std::vector l_motors_cfg; - std::vector r_motors_cfg; - std::vector waist_motors_cfg; - std::vector head_motors_cfg; - - // 左臂 if (left_arm_enabled_) { - 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>(); + 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 >(); } - // 右臂 + + auto r_can_cfg = can_cfg.getChild("RightArmCan"); + right_arm_enabled_ = readEnable(r_can_cfg); if (right_arm_enabled_) { - 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>(); + 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 >(); } - // 腰部 + + auto waist_can_cfg = can_cfg.getChild("WaistCan"); + waist_enabled_ = readEnable(waist_can_cfg); if (waist_enabled_) { - 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>(); + 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 >(); } - // 头部 + auto head_can_cfg = can_cfg.getChild("HeadCan"); + head_enabled_ = readEnable(head_can_cfg); if (head_enabled_) { - 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>(); + 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 >(); } - // 初始化 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; - for (auto &limb : limbs) { + std::vector > tasks; + + 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) @@ -122,16 +124,19 @@ void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) { 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& motor_cfg : limb.motor_cfgs) { - auto motor = std::make_shared(motor_cfg); + 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); if (protocol) motor->setProtocol(protocol); motor->init(); motor_manager_->addMotor(motor); @@ -140,26 +145,12 @@ void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) { } } - 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 096664a0..1fcce739 100644 --- a/cmvr-es/device_manager/include/device_factory.h +++ b/cmvr-es/device_manager/include/device_factory.h @@ -28,7 +28,15 @@ 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 865d1104..ee782e5d 100644 --- a/cmvr-es/device_manager/include/device_manager.h +++ b/cmvr-es/device_manager/include/device_manager.h @@ -41,6 +41,7 @@ namespace cmvr::device { DeviceManager(const DeviceManager&) = delete; DeviceManager& operator=(const DeviceManager&) = delete; + static DeviceManager& getInstance(const XmlNode &cfg); static DeviceManager& getInstance(); static void destroyInstance(); @@ -67,7 +68,7 @@ namespace cmvr::device { std::unique_ptr dev_factory_; std::unique_ptr sys_monitor_; - explicit DeviceManager(); + explicit DeviceManager(const XmlNode &cfg); 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 f80e53f4..fba92e51 100644 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ b/cmvr-es/device_manager/src/device_factory.cpp @@ -18,16 +18,172 @@ //#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) { @@ -88,38 +244,6 @@ 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"); @@ -129,4 +253,3 @@ 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 93ff9534..805516f9 100644 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ b/cmvr-es/device_manager/src/device_manager.cpp @@ -24,9 +24,12 @@ std::shared_ptr DeviceManager::instance_ = nullptr; std::once_flag DeviceManager::init_flag_; -DeviceManager::DeviceManager() { +DeviceManager::DeviceManager(const XmlNode& cfg) { 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(); @@ -39,12 +42,17 @@ DeviceManager::DeviceManager() { } } -DeviceManager& DeviceManager::getInstance() { - if (!instance_) - std::call_once(init_flag_, [&] { - instance_.reset(new DeviceManager()); +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."); + } return *instance_; } @@ -191,38 +199,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); @@ -288,36 +296,20 @@ void DeviceManager::init_devices_() { std::get>(devices_[id])->init(); } - 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(); + auto robot_node = dmgr_node.getChild("Robot"); + for (auto &node: robot_node.getChildren()){ + string id = node.getAttrString("id"); if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + 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); } - 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; @@ -354,21 +346,22 @@ void DeviceManager::init_devices_() { std::get>(devices_[id])->init(); } - 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); + + 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); } - 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 09b774ff..93e0eb20 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -58,7 +58,6 @@ 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 08c0953f..52b34d5d 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,7 +18,6 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const XmlNode &config); - BioHeadRobot(const cmvr::config::BioHeadConfig& config); ~BioHeadRobot() override = default; // 接口实现 @@ -67,7 +66,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 8ff9e3b3..c3b67887 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -26,219 +26,6 @@ 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 6fdaa099..803ee75a 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(); + auto &dmgr = DeviceManager::getInstance(dmgr_cfg); 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 6fce7f5e..ca0399d0 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -54,7 +54,6 @@ 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 28779a06..aef0af2c 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,17 +35,6 @@ 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 04d57c86..3de5b42c 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,7 +26,6 @@ #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 { @@ -37,7 +36,6 @@ 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; @@ -75,8 +73,6 @@ 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 3b7669c9..532924d2 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 bb89a415..710f88d2 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -41,7 +41,6 @@ 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 deleted file mode 100644 index abb62da2..00000000 --- a/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index 4e5edace..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp +++ /dev/null @@ -1,652 +0,0 @@ -// -// 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 deleted file mode 100644 index 11401e97..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h +++ /dev/null @@ -1,106 +0,0 @@ -// -// 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 deleted file mode 100644 index d9e2f043..00000000 --- a/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// -// 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 deleted file mode 100644 index e94979e9..00000000 --- a/cmvr-es/devices/motor/eyou_motor/eyou_motor.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// 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 6b49aa06..634a715c 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,8 +15,7 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - ETHERCAT = 2, - CUSTOM = 3 + CUSTOM = 2 }; 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 65f44ddc..cfc9bde9 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h @@ -1,12 +1,11 @@ // // 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 中完成 */ @@ -23,15 +22,6 @@ 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 91daf0ae..5759c3af 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -41,7 +41,6 @@ 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 9cd2d11d..7770997b 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -35,8 +35,6 @@ #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{ @@ -51,10 +49,12 @@ 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,7 +232,6 @@ 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 56c7542b..0146c053 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,87 +49,6 @@ 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(); @@ -140,7 +59,7 @@ void HumanoidRobot::init() { std::vector(7, 1.0)); - ik_solver_ = std::make_shared(urdf_path_, + ik_solver_ = std::make_shared("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", "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 1fba8212..d1b47e4e 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(); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); 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(); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); 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(); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); 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(); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); 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(); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); auto robot = dmgr.getDevice("hc01"); auto camera = dmgr.getDevice("right_hand_cam"); @@ -990,21 +990,3 @@ 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 dd88b4d3..ab0f76d3 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -9,13 +9,27 @@ #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[]) { - cmvr::config::ServerConfig server_config; - cmvr::ConfigHelper::getServerConfig(server_config); - ServerRunner runner(server_config); - runner.start(); + 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); runner.join(); google::ShutdownGoogleLogging(); return 0; diff --git a/cmvr-es/monitor/abstract_monitor.h b/cmvr-es/monitor/abstract_monitor.h index 244e5722..6b707e9c 100644 --- a/cmvr-es/monitor/abstract_monitor.h +++ b/cmvr-es/monitor/abstract_monitor.h @@ -13,7 +13,6 @@ 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 a22256b3..89664362 100644 --- a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h +++ b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h @@ -8,21 +8,17 @@ #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_; // 保护共享资源的互斥锁 @@ -31,4 +27,6 @@ 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 9f4497cf..20630615 100644 --- a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp +++ b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp @@ -14,6 +14,7 @@ using namespace cmvr::monitor; namespace fs = std::filesystem; using namespace std::chrono; + // 文件信息结构体 struct FileInfo { std::string path; @@ -37,40 +38,51 @@ 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; + // 删除最早创建的文件 + std::string oldestFilePath = files[0].path; if (unlink(oldestFilePath.c_str()) == -1) { std::cerr << "Failed to delete file: " << oldestFilePath << std::endl; return false; @@ -80,11 +92,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) { @@ -95,7 +107,9 @@ 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++; } @@ -105,8 +119,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"); @@ -124,42 +138,13 @@ 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; @@ -169,21 +154,28 @@ 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; @@ -191,6 +183,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 cdd879a3..a0dbc900 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 "cmvr/config/monitor_config/monitor_config.pb.h" + +#include "../../monitor/abstract_monitor.h" namespace cmvr::monitor { @@ -14,17 +14,14 @@ 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 422ece1f..8c53a401 100644 --- a/cmvr-es/monitor_manager/include/monitor_manager.h +++ b/cmvr-es/monitor_manager/include/monitor_manager.h @@ -15,38 +15,23 @@ 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: - // 原有的XML方式 - explicit MonitorManager(const XmlNode &cfg); - - // 新增proto方式 - explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config); - - void init_monitors(); // XML方式 - void init_monitors_from_proto(); // Proto方式 - + explicit MonitorManager (const XmlNode &cfg); + void init_monitors(); private: static std::once_flag init_flag_; static std::shared_ptr instance_; - // 两种配置方式，只有一个有效 - bool use_proto_ = false; - XmlNode cfg_; // XML配置 - cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置 - + XmlNode cfg_; 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 e770f294..821373e2 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,30 +17,8 @@ std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { return std::make_shared(cfg); } else { - LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; - throw runtime_error("[MonitorFactory]: Unsupported monitor type"); + LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; + throw runtime_error("[DeviceFactory]: Unsupported device 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 dde9b386..2dd9e2cf 100644 --- a/cmvr-es/monitor_manager/src/monitor_manager.cpp +++ b/cmvr-es/monitor_manager/src/monitor_manager.cpp @@ -3,7 +3,8 @@ // #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; @@ -11,19 +12,11 @@ using namespace cmvr::monitor; std::shared_ptr MonitorManager::instance_ = nullptr; std::once_flag MonitorManager::init_flag_; -// XML方式构造函数 -MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) { +MonitorManager::MonitorManager(const XmlNode& 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)); @@ -31,17 +24,9 @@ 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() first."); + throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first."); } return *instance_; } @@ -50,14 +35,13 @@ 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("[MonitorManager]: Factory returned nullptr for monitor ID: " + id); + throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); } monitors_[id] = monitor; } @@ -65,31 +49,5 @@ 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 e148c811..be8eadf8 100644 --- a/cmvr-es/service/grpc/include/server_runner.h +++ b/cmvr-es/service/grpc/include/server_runner.h @@ -11,14 +11,13 @@ #include #include #include "rapidxml/xml_parser.h" -#include "cmvr/config/server_config/server_config.pb.h" namespace cmvr::service { class ServerRunner { public: ServerRunner(); - ServerRunner(const cmvr::config::ServerConfig& config); + explicit ServerRunner(XmlNode cfg); ~ServerRunner(); ServerRunner(const ServerRunner&) = delete; @@ -26,7 +25,7 @@ namespace cmvr::service { ServerRunner(ServerRunner&&) = delete; ServerRunner& operator=(ServerRunner&&) = delete; - void start(); + void start(XmlNode cfg); void stop(); void join(); @@ -41,7 +40,7 @@ namespace cmvr::service { std::condition_variable cv_; std::thread worker_; - cmvr::config::ServerConfig cfg_; + XmlNode 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 0c4e8469..487fe705 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/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + pinocchio_ik_ = std::make_shared("/home/linbo/cmvr/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 ec04091a..dc689aff 100644 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -25,21 +25,23 @@ using namespace cmvr::service; ServerRunner::ServerRunner() = default; -ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config) -{ - // start(); +ServerRunner::ServerRunner(XmlNode cfg) { + start(std::move(cfg)); } + ServerRunner::~ServerRunner() { stop(); join(); } -void ServerRunner::start() { +void ServerRunner::start(XmlNode cfg) { 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; @@ -95,15 +97,25 @@ 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(); - std::string port = cfg_.grpc_server().port(); + 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 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 deleted file mode 100644 index 80c7d052..00000000 --- a/protos/cmvr/config/biohead_config/biohead_config.proto +++ /dev/null @@ -1,44 +0,0 @@ -// 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 deleted file mode 100644 index 368c4660..00000000 --- a/protos/cmvr/config/monitor_config/monitor_config.proto +++ /dev/null @@ -1,22 +0,0 @@ -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 deleted file mode 100644 index 1838f1eb..00000000 --- a/protos/cmvr/config/robot_config/robot_config.proto +++ /dev/null @@ -1,88 +0,0 @@ -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 deleted file mode 100644 index 911b12ca..00000000 --- a/protos/cmvr/config/server_config/server_config.proto +++ /dev/null @@ -1,10 +0,0 @@ -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 cc0986ba..9a4d60e2 100644 --- a/request.txt +++ b/request.txt @@ -33,6 +33,5 @@ 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