SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Revert "update proto config"

Browse Source 
This reverts commit 9fb185b5a8.
This commit is contained in:
linbo 2026-06-02 11:31:42 +08:00
parent 9fb185b5a8
commit fb304df94c
54 changed files with 580 additions and 2022 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cmvr-es/applications/src/touch_screen_app_test.cpp
View File

@ -21,7 +21,7 @@ void run_touch_once(int u, int v) {
const XmlNode config(kConfigPath); const XmlNode config(kConfigPath);
// auto& dm = cmvr::device::DeviceManager::getInstance(); // auto& dm = cmvr::device::DeviceManager::getInstance();
cmvr::device::DeviceManager::getInstance(); cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
// cmvr::service::ServerRunner runner; // cmvr::service::ServerRunner runner;

27
cmvr-es/common/config/biohead_config/biohead_config.pb.txt
View File

@ -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"
}
}

152
cmvr-es/common/config/cabin_robot.xml
View File

@ -0,0 +1,152 @@
<CMVR-ES>
<Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
<Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
<DeviceManager name="cmvr_es" ver="0.1" description="cmvr edge system version 0.1">
<Devices>
<AGV>
</AGV>
<Battery>
</Battery>
<Camera>
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera>
<DexHand>
<!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
</DexHand>
<Robot>
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />-->
<!-- <Neck/>-->
<Humanoid id="hc01" dof="14"
urdf="/home/lgv/cmvr/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">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="false">
<Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</HeadCan>
<WaistCan id = " " devId = " " channelId ="3" enable="false">
<Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</WaistCan>
</CanManger>
</Humanoid>
</Robot>
<BioHead>
<esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">
<!-- 眉毛 -->
<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"/>
</esp32>
</BioHead >
<Microphone>
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
</Microphone>
<Speaker>
<ffmpegSpeaker id="spk1" serial="" alas="default" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="100"/>
</Speaker>
<Canbus>
<!-- <rightArmCan id="can1" brand="SOCKET_CAN_RAW" type="USB_CARD" channel_id="CHANNEL_ID_ZERO" interface="NATIVE" baudrate="BCAN_BAUDRATE_500K"/>-->
</Canbus>
</Devices>
<HighLevelController>
<BioHeadExpre headId="bio_head" />
<CartesianWBC urdf="" />
<ScreenTouch robotID="" DexhandID="" />
</HighLevelController>
</DeviceManager>
<MonitorManager>
<DiskMonitor id="file_monitor" freq="1">
<!-- <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
<!-- <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
<!-- <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
<!-- <Folder fileDir="../log" maxVolume="1000"/>-->
</DiskMonitor>
<JointMonitor id="robot_joint_monitor" freq="200">
<RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
</JointMonitor>
</MonitorManager>
<gRPCServer port="50052">
</gRPCServer>
</CMVR-ES>

2
cmvr-es/common/config/camera_config/camera_config.pb.txt
View File

@ -27,7 +27,7 @@ realsense_cameras {
align_mode: ALIGN_MODE_COLOR align_mode: ALIGN_MODE_COLOR
buffer_size: 30 buffer_size: 30
sync: false sync: false
enable: false enable: true
} }
realsense_cameras { realsense_cameras {

4
cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt
View File

@ -1,6 +1,6 @@
rh56dftp_dexhands { rh56dftp_dexhands {
id: "hand1" id: "hand1"
ip: "192.168.1.223" ip: "192.168.1.213"
port: 6000 port: 6000
poll_interval_ms: 10 poll_interval_ms: 10
enable: false enable: false
@ -11,7 +11,7 @@ rh56dftp_dexhands {
ip: "192.168.1.224" ip: "192.168.1.224"
port: 6000 port: 6000
poll_interval_ms: 10 poll_interval_ms: 10
enable: false enable: true
} }
px_6ax_gen3 { px_6ax_gen3 {

25
cmvr-es/common/config/monitor_config/monitor_config.pb.txt
View File

@ -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
}
}
}

212
cmvr-es/common/config/robot_config/robot_config.pb.txt
View File

@ -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
}
}

3
cmvr-es/common/config/server_config/server_config.pb.txt
View File

@ -1,3 +0,0 @@
grpc_server {
port: "50052"
}

17
cmvr-es/common/utils/config_helper/include/config_helper.h
View File

@ -10,9 +10,6 @@
#include "cmvr/config/microphone_config/microphone_config.pb.h" #include "cmvr/config/microphone_config/microphone_config.pb.h"
#include "cmvr/config/speaker_config/speaker_conifg.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/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) \ #define GET_CONFIG(file, para) \
([&]() -> bool { \ ([&]() -> bool { \
@ -85,20 +82,6 @@ namespace cmvr
return SET_CONFIG(config, touch_screen_app_config_file); 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: private:
// Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx) // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx)
template <class T> template <class T>

3
cmvr-es/common/utils/config_helper/include/config_setting.h
View File

@ -6,6 +6,3 @@ DECLARE_string(dexhand_config_file);
DECLARE_string(microphone_config_file); DECLARE_string(microphone_config_file);
DECLARE_string(speaker_config_file); DECLARE_string(speaker_config_file);
DECLARE_string(touch_screen_app_config_file); DECLARE_string(touch_screen_app_config_file);
DECLARE_string(biohead_config_file);
DECLARE_string(robot_config_file);
DECLARE_string(server_config_file);

12
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -88,15 +88,3 @@ DEFINE_string(speaker_config_file,
DEFINE_string(touch_screen_app_config_file, DEFINE_string(touch_screen_app_config_file,
basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt", basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
"The configuration file for TouchScreenApp"); "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");

1
cmvr-es/data_center/CMakeLists.txt
View File

@ -11,7 +11,6 @@ target_link_libraries(data_center PRIVATE
cmvr_es::utils cmvr_es::utils
cmvr_es::device::canbus cmvr_es::device::canbus
cmvr_es::device::ti5motor cmvr_es::device::ti5motor
cmvr_es::device::eyoumotor
protobuf protobuf
glog glog
) )

4
cmvr-es/data_center/include/motors_info.h
View File

@ -23,8 +23,8 @@ namespace cmvr {
public: public:
~MotorsInfo(){}; ~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 *getInstance() {
static MotorsInfo instance; static MotorsInfo instance;

119
cmvr-es/data_center/src/motors_info.cpp
View File

@ -3,8 +3,6 @@
// //
#include "data_center/include/motors_info.h" #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/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h" #include "motor/ti5_motor/ti5_motor.h"
@ -14,70 +12,58 @@ using namespace cmvr::device;
MotorsInfo::MotorsInfo() { MotorsInfo::MotorsInfo() {
} }
void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) { void MotorsInfo::init(const XmlNode &cfg) {
const auto& can_manager = config.can_manager(); // 定义 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状态 auto l_can_cfg = can_cfg.getChild("LeftArmCan");
left_arm_enabled_ = can_manager.left_arm().enable(); left_arm_enabled_ = readEnable(l_can_cfg);
right_arm_enabled_ = can_manager.right_arm().enable();
waist_enabled_ = can_manager.waist().enable();
head_enabled_ = can_manager.head().enable();
// 局部变量存储电机配置
std::vector<cmvr::config::MotorConfig> l_motors_cfg;
std::vector<cmvr::config::MotorConfig> r_motors_cfg;
std::vector<cmvr::config::MotorConfig> waist_motors_cfg;
std::vector<cmvr::config::MotorConfig> head_motors_cfg;
// 左臂
if (left_arm_enabled_) { if (left_arm_enabled_) {
const auto& left_arm = can_manager.left_arm(); l_motors_cfg_ = l_can_cfg.getChildren("Motor");
for (const auto& motor : left_arm.motors()) { l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
l_motors_cfg.push_back(motor);
}
l_can_client_ = std::make_shared<SocketCanClientRaw>(left_arm);
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
} }
// 右臂
auto r_can_cfg = can_cfg.getChild("RightArmCan");
right_arm_enabled_ = readEnable(r_can_cfg);
if (right_arm_enabled_) { if (right_arm_enabled_) {
const auto& right_arm = can_manager.right_arm(); r_motors_cfg_ = r_can_cfg.getChildren("Motor");
for (const auto& motor : right_arm.motors()) { r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
r_motors_cfg.push_back(motor);
}
r_can_client_ = std::make_shared<SocketCanClientRaw>(right_arm);
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
} }
// 腰部
auto waist_can_cfg = can_cfg.getChild("WaistCan");
waist_enabled_ = readEnable(waist_can_cfg);
if (waist_enabled_) { if (waist_enabled_) {
const auto& waist = can_manager.waist(); waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
for (const auto& motor : waist.motors()) { waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
waist_motors_cfg.push_back(motor);
}
waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist);
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
} }
// 头部 auto head_can_cfg = can_cfg.getChild("HeadCan");
head_enabled_ = readEnable(head_can_cfg);
if (head_enabled_) { if (head_enabled_) {
const auto& head = can_manager.head(); head_motors_cfg_ = head_can_cfg.getChildren("Motor");
for (const auto& motor : head.motors()) { head_can_client_ = std::make_shared<SocketCanClientRaw>(head_can_cfg);
head_motors_cfg.push_back(motor);
}
head_can_client_ = std::make_shared<SocketCanClientRaw>(head);
head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
} }
// 初始化 limbs // 开始初始化
struct Limb { struct Limb {
std::string name; std::string name;
bool enabled; bool enabled;
@ -85,24 +71,40 @@ void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) {
std::shared_ptr<CanSender<msgs::RobotDetail> > sender; std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver; std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager; std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
std::vector<cmvr::config::MotorConfig> motor_cfgs; std::vector<XmlNode> motor_cfgs;
}; };
std::vector<Limb> limbs{ std::vector<Limb> 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}, "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_,
{"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, r_motors_cfg}, waist_motors_cfg_
{"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, head_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<MotorManager>(); motor_manager_ = std::make_shared<MotorManager>();
std::vector<std::future<void> > tasks; std::vector<std::future<void> > tasks;
for (auto &limb: limbs) { for (auto &limb: limbs) {
if (!limb.enabled) continue; if (!limb.enabled) continue;
if (limb.client) limb.client->init(); if (limb.client) limb.client->init();
// 2. 初始化 Sender / Receiver（如果有）
if (limb.sender && limb.receiver && limb.client) { if (limb.sender && limb.receiver && limb.client) {
auto ret = limb.sender->Init(limb.client.get(), false); auto ret = limb.sender->Init(limb.client.get(), false);
if (ret != ErrorCode::OK) 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."; LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
} }
// 3. 创建协议（如果有CAN）
std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr; std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
if (limb.sender && limb.message_manager) { if (limb.sender && limb.message_manager) {
protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager); protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
} }
// 4. 并行初始化电机
if (!limb.motor_cfgs.empty()) { if (!limb.motor_cfgs.empty()) {
tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] { tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << " motors..."; LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name <<
for (const auto& motor_cfg : limb.motor_cfgs) { " motors...";
auto motor = std::make_shared<Ti5Motor>(motor_cfg); for (const auto &cfg: limb.motor_cfgs) {
auto motor = std::make_shared<Ti5Motor>(cfg);
if (protocol) motor->setProtocol(protocol); if (protocol) motor->setProtocol(protocol);
motor->init(); motor->init();
motor_manager_->addMotor(motor); 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."; LOG(INFO) << "All enabled motors initialized successfully.";
} }
void MotorsInfo::init(const cmvr::config::EthercatRobotConfig& config)
{
if (!motor_manager_)
{
motor_manager_ = std::make_shared<MotorManager>();
}
auto protocol = std::make_shared<EYouEthercatProtocol>(config.master().interface());
for (const auto& motor_cfg : config.motors()) {
auto motor = std::make_shared<EYouMotor>(motor_cfg);
if (protocol) motor->setProtocol(protocol);
motor->init();
motor_manager_->addMotor(motor);
}
}
bool MotorsInfo::getJointsQ(const std::vector<std::string> &joints_name, bool MotorsInfo::getJointsQ(const std::vector<std::string> &joints_name,
std::unordered_map<std::string, double> &joint_qs) const { std::unordered_map<std::string, double> &joint_qs) const {
auto res{true}; auto res{true};

10
cmvr-es/device_manager/include/device_factory.h
View File

@ -28,7 +28,15 @@ namespace cmvr::device {
template <typename DeviceType, typename ConfigType> template <typename DeviceType, typename ConfigType>
std::shared_ptr<DeviceType> createFromConfig(const ConfigType& cfg); std::shared_ptr<DeviceType> createFromConfig(const ConfigType& cfg);
private: private:
std::shared_ptr<AbstractAGV> create_agv_(const XmlNode& cfg);
std::shared_ptr<AbstractBattery> create_battery_(const XmlNode& cfg);
std::shared_ptr<AbstractCamera> create_camera_(const XmlNode& cfg);
std::shared_ptr<AbstractDexHand> create_dexhand_(const XmlNode& cfg);
std::shared_ptr<AbstractGripper> create_gripper_(const XmlNode& cfg);
std::shared_ptr<AbstractMicrophone> create_mic_(const XmlNode& cfg);
std::shared_ptr<AbstractRobot> create_robot_(const XmlNode& cfg);
std::shared_ptr<AbstractSpeaker> create_speaker_(const XmlNode& cfg);
std::shared_ptr<AbstractBiohead> create_biohead_(const XmlNode& cfg);
}; };
} }

3
cmvr-es/device_manager/include/device_manager.h
View File

@ -41,6 +41,7 @@ namespace cmvr::device {
DeviceManager(const DeviceManager&) = delete; DeviceManager(const DeviceManager&) = delete;
DeviceManager& operator=(const DeviceManager&) = delete; DeviceManager& operator=(const DeviceManager&) = delete;
static DeviceManager& getInstance(const XmlNode &cfg);
static DeviceManager& getInstance(); static DeviceManager& getInstance();
static void destroyInstance(); static void destroyInstance();
@ -67,7 +68,7 @@ namespace cmvr::device {
std::unique_ptr<DeviceFactory> dev_factory_; std::unique_ptr<DeviceFactory> dev_factory_;
std::unique_ptr<device::SystemMonitor> sys_monitor_; std::unique_ptr<device::SystemMonitor> sys_monitor_;
explicit DeviceManager(); explicit DeviceManager(const XmlNode &cfg);
void init_devices_(); void init_devices_();
void get_os_info_(); void get_os_info_();
}; };

197
cmvr-es/device_manager/src/device_factory.cpp
View File

@ -18,16 +18,172 @@
//#include "devices/robot/aubo_robot/include/aubo_robot.h" //#include "devices/robot/aubo_robot/include/aubo_robot.h"
using namespace std; using namespace std;
using namespace cmvr::device; using namespace cmvr::device;
template std::shared_ptr<AbstractAGV> DeviceFactory::create<AbstractAGV>(const XmlNode&);
template std::shared_ptr<AbstractBattery> DeviceFactory::create<AbstractBattery>(const XmlNode&);
template std::shared_ptr<AbstractCamera> DeviceFactory::create<AbstractCamera>(const XmlNode&);
template std::shared_ptr<AbstractDexHand> DeviceFactory::create<AbstractDexHand>(const XmlNode&);
template std::shared_ptr<AbstractGripper> DeviceFactory::create<AbstractGripper>(const XmlNode&);
template std::shared_ptr<AbstractMicrophone> DeviceFactory::create<AbstractMicrophone>(const XmlNode&);
template std::shared_ptr<AbstractRobot> DeviceFactory::create<AbstractRobot>(const XmlNode&);
template std::shared_ptr<AbstractSpeaker> DeviceFactory::create<AbstractSpeaker>(const XmlNode&);
template std::shared_ptr<AbstractBiohead> DeviceFactory::create<AbstractBiohead>(const XmlNode&);
template <typename DeviceType>
std::shared_ptr<DeviceType> DeviceFactory::create(const XmlNode& cfg) {
if constexpr (std::is_same_v<DeviceType, AbstractAGV>) {
return create_agv_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractBattery>) {
return create_battery_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractCamera>) {
return create_camera_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractDexHand>) {
return create_dexhand_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractGripper>) {
return create_gripper_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractMicrophone>) {
return create_mic_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractRobot>) {
return create_robot_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractSpeaker>) {
return create_speaker_(cfg);
}else if constexpr (std::is_same_v<DeviceType, AbstractBiohead>){
return create_biohead_(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[DeviceFactory]: Unsupported device type");
}
}
std::shared_ptr<AbstractAGV> DeviceFactory::create_agv_(const XmlNode& cfg) {
return nullptr;
}
std::shared_ptr<AbstractBattery> DeviceFactory::create_battery_(const XmlNode& cfg) {
return nullptr;
}
std::shared_ptr<AbstractCamera> DeviceFactory::create_camera_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "UVCCamera") {
return std::make_shared<UVCCamera>(cfg);
}
else if (cfg.getNodeName() == "MechMind") {
//return std::make_shared<MechmindCamera>(cfg);
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
else if (cfg.getNodeName() == "RealsenseCamera") {
return std::make_shared<RealsenseCamera>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
}
catch (const exception &e) {
return nullptr;
}
}
std::shared_ptr<AbstractDexHand> DeviceFactory::create_dexhand_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "RH56DFTP") {
return std::make_shared<RH56DFTPDexhand>(cfg);
}
else if (cfg.getNodeName() == "PX6AXGen3") {
return std::make_shared<PX6AXGen3>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
}
catch (const exception &e) {
return nullptr;
}
}
std::shared_ptr<AbstractGripper> DeviceFactory::create_gripper_(const XmlNode& cfg) {
return nullptr;
}
std::shared_ptr<AbstractMicrophone> DeviceFactory::create_mic_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "ffmpegMicPhone") {
return std::make_shared<ffmpegMicroPhone>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
}
catch (const exception &e) {
return nullptr;
}
return nullptr;
}
std::shared_ptr<AbstractRobot> DeviceFactory::create_robot_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "Humanoid") {
auto motos_info = MotorsInfo::getInstance();
motos_info->init(cfg);
return std::make_shared<HumanoidRobot<14>>(cfg);
}
else if (cfg.getNodeName() == "AuboRobot") {
//return std::make_shared<AuboRobot<6>>(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_ptr<AbstractBiohead>DeviceFactory::create_biohead_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "esp32") {
return std::make_shared<BioHeadRobot>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
}
catch (const exception &e) {
return nullptr;
}
}
std::shared_ptr<AbstractSpeaker> DeviceFactory::create_speaker_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "ffmpegSpeaker") {
return std::make_shared<ffmpegSpeaker>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;
}
}
catch (const exception &e) {
return nullptr;
}
}
template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::UVCCameraConfig& cfg); template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::UVCCameraConfig& cfg);
template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::RealSenseCameraConfig& cfg); template std::shared_ptr<AbstractCamera> DeviceFactory::createFromConfig<AbstractCamera>(const config::RealSenseCameraConfig& cfg);
template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::RH56DFTPDexHandConfig& cfg); template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::RH56DFTPDexHandConfig& cfg);
template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::PX6AXGen3& cfg); template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::PX6AXGen3& cfg);
template std::shared_ptr<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& cfg); template std::shared_ptr<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& cfg);
template std::shared_ptr<AbstractSpeaker> DeviceFactory::createFromConfig<AbstractSpeaker>(const config::FFMpegSpeakerConfig& cfg); template std::shared_ptr<AbstractSpeaker> DeviceFactory::createFromConfig<AbstractSpeaker>(const config::FFMpegSpeakerConfig& cfg);
template std::shared_ptr<AbstractBiohead> DeviceFactory::createFromConfig<AbstractBiohead>(const config::BioHeadConfig& cfg);
template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::HumanRobotConfig& cfg);
template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::EthercatRobotConfig& cfg);
template <typename DeviceType, typename ConfigType> template <typename DeviceType, typename ConfigType>
std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cfg) std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cfg)
{ {
@ -88,38 +244,6 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
} }
} }
else if constexpr (std::is_same_v<DeviceType, AbstractBiohead>)
{
if constexpr (std::is_same_v<ConfigType, config::BioHeadConfig>)
{
return std::make_shared<BioHeadRobot>(cfg);
}
else
{
LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type ";
throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
}
}
else if constexpr (std::is_same_v<DeviceType, AbstractRobot>)
{
if constexpr (std::is_same_v<ConfigType, config::HumanRobotConfig>)
{
auto motos_info = MotorsInfo::getInstance();
motos_info->init(cfg);
return std::make_shared<HumanoidRobot<14>>(cfg);
}
else if constexpr (std::is_same_v<ConfigType, config::EthercatRobotConfig>)
{
auto motos_info = MotorsInfo::getInstance();
motos_info->init(cfg);
return std::make_shared<HumanoidRobot<14>>(cfg);
}
else
{
LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type ";
throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
}
}
else { else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[DeviceFactory]: Unsupported device type"); throw runtime_error("[DeviceFactory]: Unsupported device type");
@ -129,4 +253,3 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
return nullptr; return nullptr;
} }
} }

141
cmvr-es/device_manager/src/device_manager.cpp
View File

@ -24,9 +24,12 @@ std::shared_ptr<DeviceManager> DeviceManager::instance_ = nullptr;
std::once_flag DeviceManager::init_flag_; std::once_flag DeviceManager::init_flag_;
DeviceManager::DeviceManager() { DeviceManager::DeviceManager(const XmlNode& cfg) {
try { 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_(); get_os_info_();
dev_factory_ = std::make_unique<DeviceFactory>(); dev_factory_ = std::make_unique<DeviceFactory>();
@ -39,12 +42,17 @@ DeviceManager::DeviceManager() {
} }
} }
DeviceManager& DeviceManager::getInstance() { DeviceManager& DeviceManager::getInstance(const XmlNode& cfg) {
if (!instance_) std::call_once(init_flag_, [&cfg] {
std::call_once(init_flag_, [&] { instance_.reset(new DeviceManager(cfg));
instance_.reset(new DeviceManager());
}); });
return *instance_;
}
DeviceManager& DeviceManager::getInstance() {
if (!instance_) {
throw std::runtime_error("[DeviceManager] (getInstance): DeviceManager not initialized. Call getInstance(const XmlNode&) first.");
}
return *instance_; return *instance_;
} }
@ -191,38 +199,38 @@ void DeviceManager::getSystemStatus(SystemStatus& status) const {
void DeviceManager::init_devices_() { void DeviceManager::init_devices_() {
try{ try{
// auto dmgr_node = cfg_.getChild("Devices"); auto dmgr_node = cfg_.getChild("Devices");
// auto battery_node = dmgr_node.getChild("Battery"); auto battery_node = dmgr_node.getChild("Battery");
// for (auto &node: battery_node.getChildren()){ for (auto &node: battery_node.getChildren()){
// string id = node.getAttrString("id"); string id = node.getAttrString("id");
// if (devices_.count(id)) { if (devices_.count(id)) {
// LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id; LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id;
// throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id); throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id);
// } }
// auto device = dev_factory_->create<AbstractBattery>(node); auto device = dev_factory_->create<AbstractBattery>(node);
// if (device == nullptr) { if (device == nullptr) {
// throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
// } }
// devices_[id] = device; devices_[id] = device;
// std::get<std::shared_ptr<AbstractBattery>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractBattery>>(devices_[id])->init();
// LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success"; LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success";
// } }
// auto agv_node = dmgr_node.getChild("AGV"); auto agv_node = dmgr_node.getChild("AGV");
// for (auto &node: agv_node.getChildren()){ for (auto &node: agv_node.getChildren()){
// string id = node.getAttrString("id"); string id = node.getAttrString("id");
// if (devices_.count(id)) { if (devices_.count(id)) {
// LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id; LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id;
// throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id); throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id);
// } }
// auto device = dev_factory_->create<AbstractAGV>(node); auto device = dev_factory_->create<AbstractAGV>(node);
// if (device == nullptr) { if (device == nullptr) {
// throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
// } }
// devices_[id] = device; devices_[id] = device;
// std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init();
// LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success"; LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success";
// } }
config::CameraConfig camera_config; config::CameraConfig camera_config;
ConfigHelper::getCamerasConfig(camera_config); ConfigHelper::getCamerasConfig(camera_config);
@ -288,36 +296,20 @@ void DeviceManager::init_devices_() {
std::get<std::shared_ptr<AbstractDexHand>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractDexHand>>(devices_[id])->init();
} }
config::RobotConfig robot_config; auto robot_node = dmgr_node.getChild("Robot");
ConfigHelper::getRobotConfig(robot_config); for (auto &node: robot_node.getChildren()){
for (int i = 0; i < robot_config.humanrobots().size(); i++) { string id = node.getAttrString("id");
auto robot = robot_config.humanrobots(i);
if (!robot.enable())
continue;
auto id = robot.id();
if (devices_.count(id)) { if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id; LOG(ERROR) << "[DeviceManager]: Duplicate robot Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id); throw runtime_error("[DeviceManager]: Duplicate robot Device ID" + id);
}
auto device = dev_factory_->create<AbstractRobot>(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<AbstractRobot,config::HumanRobotConfig>(robot);
devices_[id] = device;
std::get<std::shared_ptr<AbstractRobot>>(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<AbstractRobot,config::EthercatRobotConfig>(robot);
devices_[id] = device; devices_[id] = device;
std::get<std::shared_ptr<AbstractRobot>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractRobot>>(devices_[id])->init();
LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success";
} }
config::MicroPhoneConfig micro_phone_config; config::MicroPhoneConfig micro_phone_config;
@ -354,21 +346,22 @@ void DeviceManager::init_devices_() {
std::get<std::shared_ptr<AbstractSpeaker>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractSpeaker>>(devices_[id])->init();
} }
config::HeadConfig head_cfg;
ConfigHelper::getHeadConfig(head_cfg); auto biohead_node = dmgr_node.getChild("BioHead");
for (int i = 0; i < head_cfg.bioheads().size(); i++) { for (auto &node: biohead_node.getChildren()){
auto head = head_cfg.bioheads(i); string id = node.getAttrString("id");
if (!head.enable())
continue;
auto id = head.id();
if (devices_.count(id)){ if (devices_.count(id)){
LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id; LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id); throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id);
}
auto device = dev_factory_->create<AbstractBiohead>(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<AbstractBiohead,config::BioHeadConfig>(head);
devices_[id] = device; devices_[id] = device;
std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init(); std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init();
LOG(INFO) << "[BioHead]: Init biohead " << id << " Success";
} }
} }
catch (const exception& e) { catch (const exception& e) {

1
cmvr-es/devices/biohead/abstract_biohead.h
View File

@ -58,7 +58,6 @@ namespace cmvr::device {
// 抽象头部类 // 抽象头部类
class AbstractBiohead : public AbstractDevice { class AbstractBiohead : public AbstractDevice {
public: public:
AbstractBiohead() = default;
explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {} explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {}
~AbstractBiohead() override = default; ~AbstractBiohead() override = default;

5
cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h
View File

@ -7,7 +7,7 @@
#include <string> #include <string>
#include <memory> #include <memory>
#include <mutex> #include <mutex>
#include "cmvr/config/biohead_config/biohead_config.pb.h"
namespace cmvr::device { namespace cmvr::device {
struct ServoChannel { struct ServoChannel {
@ -18,7 +18,6 @@ namespace cmvr::device {
class BioHeadRobot : public AbstractBiohead { class BioHeadRobot : public AbstractBiohead {
public: public:
explicit BioHeadRobot(const XmlNode &config); explicit BioHeadRobot(const XmlNode &config);
BioHeadRobot(const cmvr::config::BioHeadConfig& config);
~BioHeadRobot() override = default; ~BioHeadRobot() override = default;
// 接口实现 // 接口实现
@ -67,7 +66,7 @@ namespace cmvr::device {
std::shared_ptr<std::thread> speak_thread_; std::shared_ptr<std::thread> speak_thread_;
std::atomic<bool> speak_running_{false}; std::atomic<bool> speak_running_{false};
cmvr::config::BioHeadConfig config_;
}; };

213
cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
View File

@ -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<SerialPort>();
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<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> 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<double> 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<uint8_t>(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<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> 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<double> 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<uint8_t>(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<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> 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<double> 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<uint8_t>(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) { void BioHeadRobot::parseXmlConfig(const XmlNode &node) {

2
cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp
View File

@ -21,7 +21,7 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
return; return;
} }
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(); auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto cam = dmgr.getDevice<AbstractCamera>("cam4"); auto cam = dmgr.getDevice<AbstractCamera>("cam4");
cam->start(); cam->start();

1
cmvr-es/devices/canbus/abstract_canbus.h
View File

@ -54,7 +54,6 @@ namespace cmvr::device {
class AbstractCanbus : public AbstractDevice { class AbstractCanbus : public AbstractDevice {
public: public:
AbstractCanbus() = default;
AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {} AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {}
~AbstractCanbus() {} ~AbstractCanbus() {}
/** /**

11
cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc
View File

@ -35,17 +35,6 @@ namespace cmvr {
// LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what(); // 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<CANCardParameter::CANChannelId>(channel_id);
interface_ = CANCardParameter::NATIVE;
enable_can_err_check_ = false;
}
void SocketCanClientRaw::init() { void SocketCanClientRaw::init() {
// if (!parameter.has_channel_id()) { // if (!parameter.has_channel_id()) {

4
cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h
View File

@ -26,7 +26,6 @@
#include "gflags/gflags.h" #include "gflags/gflags.h"
#include "../../abstract_canbus.h" #include "../../abstract_canbus.h"
#include "canbus/common/canbus_consts.h" #include "canbus/common/canbus_consts.h"
#include "cmvr/config/robot_config/robot_config.pb.h"
namespace cmvr { namespace cmvr {
namespace device { namespace device {
@ -37,7 +36,6 @@ namespace cmvr {
class SocketCanClientRaw final : public AbstractCanbus { class SocketCanClientRaw final : public AbstractCanbus {
public: public:
explicit SocketCanClientRaw(const XmlNode &cfg); explicit SocketCanClientRaw(const XmlNode &cfg);
SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config);
~SocketCanClientRaw(); ~SocketCanClientRaw();
void init() override; void init() override;
void start() override; void start() override;
@ -75,8 +73,6 @@ namespace cmvr {
// //
bool enable_can_err_check_{false}; bool enable_can_err_check_{false};
cmvr::config::CanDeviceConfig config_;
}; };
} }
} }

2
cmvr-es/devices/motor/CMakeLists.txt
View File

@ -1,5 +1,5 @@
add_subdirectory(ti5_motor) add_subdirectory(ti5_motor)
add_subdirectory(eyou_motor)
# -------------------------------------------------------- # --------------------------------------------------------
# Unit test # Unit test
# -------------------------------------------------------- # --------------------------------------------------------

1
cmvr-es/devices/motor/abstract_motor.h
View File

@ -41,7 +41,6 @@ namespace cmvr::device{
} JointStatus; } JointStatus;
public: public:
AbstractMotor() = default;
explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {} explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {}
AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {} AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {}
~AbstractMotor() override = default; ~AbstractMotor() override = default;

19
cmvr-es/devices/motor/eyou_motor/CMakeLists.txt
View File

@ -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)

652
cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp
View File

@ -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 <cmath>
#include <thread>
#include <chrono>
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<device::MessageManager<msgs::RobotDetail>>();
// 初始化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<NmtResponseProtocol<RobotDetail>, 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<uint32_t>(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<int32_t>(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<uint32_t>(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<uint32_t>((std::abs(u_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
auto decel = static_cast<uint32_t>((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: "<<iRet << std::endl;
return false;
}
std::cout <<"actualPos last: "<<pos << std::endl;
// if (0 == pos)
// {
// std::cout << "actualPos is alary 0, quit" << std::endl;
// continue;
// }
iRet = eth_readSDO(node_id,0x607c,0x00,&offset,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"read home offset failed: "<<iRet << std::endl;
return false;
}
std::cout <<"home offset before: " << offset << std::endl;
//先将home offset设为0
offset = 0;
iRet = eth_writeSDO(node_id,0x607c,0x00,&offset,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"writeSdo offset 0 failed: "<<iRet << std::endl;
return false;
}
std::cout <<"writeSdo offset 0 successed" << std::endl;
//std::this_thread::sleep_for(std::chrono::milliseconds(ms));
std::this_thread::sleep_for(std::chrono::milliseconds(500)); //等待home offset设置生效
//读取实际位置
iRet = eth_readSDO(node_id,0x6064,0x00,&pos,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"eth_getActualPosition failed: "<<iRet << std::endl;
return false;
}
std::cout <<"actualPos before: "<< pos << std::endl;
//获取轴转数，超圈处理
iRet = eth_readSDO(node_id,0x6091,0x02,&rev,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"gearRationShaftRevolution failed: "<<iRet << std::endl;
return false;
}
std::cout <<"gearRationShaftRevolution: "<<rev << std::endl;
long long r = rev;
int temp = r/2;
if(pos > 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: "<<iRet << std::endl;
return false;
}
std::cout <<"writeSdo offset -pos: "<<offset << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500)); //等待home offset设置生效
//读取实际位置
iRet = eth_readSDO(node_id,0x6064,0x00,&pos,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"eth_getActualPosition failed: "<<iRet << std::endl;
return false;
}
std::cout <<"actualPos after: "<< pos << std::endl;
//set storeParameters 保存
huint32 value = 0x65766173;
iRet = eth_writeSDO(node_id,0x1010,0x01,&value,eth_DataType_uint32,20000);
if (iRet != ETH_SUCCESS)
{
std::cout <<"storeParameters set failed: "<<iRet << std::endl;
return false;
}
std::cout <<"storeParameters set: "<<value << std::endl;
LOG(INFO) << "[EYouEthercatProtocol] Motor " << node_id << " calibrated successfully";
return true;
} catch (const std::exception& e) {
LOG(ERROR) << "[EYouEthercatProtocol] Calibration failed for node " << node_id << ": " << e.what();
return false;
}
}
// 刹车
void EYouEthercatProtocol::brake(uint8_t node_id) {
// 直接调用失能还是快速停机呢？
eth_quickStop(node_id);
// eth_enable(node_id);
LOG(INFO) << "[EYouEthercatProtocol] Brake applied to motor " << node_id;
}
// 检查是否到达目标位置
bool EYouEthercatProtocol::reachedTargetQ(uint8_t node_id) {
uint16_t status_word = 0;
if (eth_getStatusWord(node_id, &status_word) != ETH_SUCCESS) {
return false;
}
// 检查状态字的bit 10（目标到达位）
return (status_word & 0x0400) != 0;
}
// 获取当前位置
double EYouEthercatProtocol::getQ(uint8_t node_id) {
int32_t position = 0;
if (eth_getActualPosition(node_id, &position) != ETH_SUCCESS) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to read position for node " << node_id;
return 0.0;
}
return posToAngle(position);
}
// 获取当前速度
double EYouEthercatProtocol::getQd(uint8_t node_id) {
int32_t velocity = 0;
if (eth_getActualVelocity(node_id, &velocity) != ETH_SUCCESS) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to read velocity for node " << node_id;
return 0.0;
}
return speedToVel(velocity);
}
// 设置速度
void EYouEthercatProtocol::setQd(uint8_t node_id, double qd) {
auto speed = static_cast<uint32_t>(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<int32_t>(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<uint32_t>((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<RobotDetail> EYouEthercatProtocol::GetRobotDetail() {
if (message_manager_) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
return data_ptr;
}
return std::make_unique<msgs::RobotDetail>();
}
// 辅助函数实现（弧度制版本）
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<int32_t>((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<bool()> 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);
// 可以根据需要更新其他状态信息
}

106
cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h
View File

@ -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 <cmath>
#include <unordered_map>
#include <functional>
#include <memory>
#include <chrono>
#include <thread>
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<msgs::RobotDetail> 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<bool()> condition, int timeout_ms);
void updateNodeState(uint8_t node_id); // 更新节点状态
// 成员变量
std::unordered_map<uint8_t, NodeState> node_states_; // 节点状态映射
std::unordered_map<uint8_t, double> last_Qd_; // 上次设置的速度
std::unordered_map<uint8_t, uint32_t> last_Qdd_; // 上次设置的加速度
std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_;
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_;
std::string interface_name_; // 网卡接口名
bool initialized_; // 是否已初始化SDK
int slave_count_; // 从站数量
};
} // namespace device
} // namespace cmvr

5
cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp
View File

@ -1,5 +0,0 @@
//
// Created by cmvr on 2026/3/30.
//
#include "eyou_motor.h"

51
cmvr-es/devices/motor/eyou_motor/eyou_motor.h
View File

@ -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<EYouEthercatProtocol>(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_);
}
}
};
}
}

3
cmvr-es/devices/motor/motor_protocol_interface.h
View File

@ -15,8 +15,7 @@ namespace cmvr {
public: public:
enum class CommProto : uint8_t { enum class CommProto : uint8_t {
CANOPEN = 1, CANOPEN = 1,
ETHERCAT = 2, CUSTOM = 2
CUSTOM = 3
}; };
virtual ~MotorProtocolInterface() = default; virtual ~MotorProtocolInterface() = default;

12
cmvr-es/devices/motor/ti5_motor/ti5_motor.h
View File

@ -1,12 +1,11 @@
// //
// Created by lgv on 2025/8/1. // Created by lgv on 2025/8/1.
// //
#pragma once
#include "../abstract_motor.h" #include "../abstract_motor.h"
#include "motor/motor_protocol_interface.h" #include "motor/motor_protocol_interface.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "cmvr/config/robot_config/robot_config.pb.h"
/** /**
* class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成 * class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成
*/ */
@ -23,15 +22,6 @@ namespace cmvr {
node_id_ = info_.id; 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;
}
/** /**
* 电机初始化在具体的类中完成 * 电机初始化在具体的类中完成
*/ */

1
cmvr-es/devices/robot/abstract_robot.h
View File

@ -41,7 +41,6 @@ namespace cmvr::device{
class AbstractRobot: public AbstractDevice { class AbstractRobot: public AbstractDevice {
public: public:
AbstractRobot() = default;
explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {} explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
~AbstractRobot() override=default; ~AbstractRobot() override=default;

9
cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h
View File

@ -35,8 +35,6 @@
#include "planner/joint_space_planner/include/joint_space_planner_creator.h" #include "planner/joint_space_planner/include/joint_space_planner_creator.h"
#include "planner/joint_space_planner/include/joint_space_planner.h" #include "planner/joint_space_planner/include/joint_space_planner.h"
#include "ik_solver/include/pinocchio_dls_ik_solver.h" #include "ik_solver/include/pinocchio_dls_ik_solver.h"
#include "cmvr/config/robot_config/robot_config.pb.h"
namespace cmvr::device{ namespace cmvr::device{
@ -51,10 +49,12 @@ namespace cmvr::device{
ROBOT_ERROR ROBOT_ERROR
} RobotStateMachine; } RobotStateMachine;
// typedef enum {
// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE
// } RobotRunningMode;
public: public:
explicit HumanoidRobot(const XmlNode& cfg); explicit HumanoidRobot(const XmlNode& cfg);
HumanoidRobot(const cmvr::config::HumanRobotConfig& config);
HumanoidRobot(const cmvr::config::EthercatRobotConfig& config);
~HumanoidRobot(); ~HumanoidRobot();
void init() override; void init() override;
@ -232,7 +232,6 @@ namespace cmvr::device{
std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false}; std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
std::string urdf_path_ = "";
}; };

85
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
View File

@ -16,7 +16,7 @@ template<int DOF>
HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
try { try {
id_ = cfg.getAttrString("id"); id_ = cfg.getAttrString("id");
urdf_path_ = cfg.getAttrString("urdf");
dof_ = DOF; dof_ = DOF;
if (!pathExists(cfg.getAttrString("urdf"))) { if (!pathExists(cfg.getAttrString("urdf"))) {
throw runtime_error("urdf file does not exist"); throw runtime_error("urdf file does not exist");
@ -49,87 +49,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
} }
} }
template<int DOF>
HumanoidRobot<DOF>::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<cmvr::dyn::Robot<DOF> >(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<ctrl::CartesianController<DOF> >(m_robot_);
upd_freq_ = config.updfreq();
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
upd_freq_ = 1000;
upd_timer_ = make_shared<FDTimer>();
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<int DOF>
HumanoidRobot<DOF>::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<cmvr::dyn::Robot<DOF> >(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<ctrl::CartesianController<DOF> >(m_robot_);
upd_freq_ = config.updfreq();
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
upd_freq_ = 1000;
upd_timer_ = make_shared<FDTimer>();
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<int DOF> template<int DOF>
void HumanoidRobot<DOF>::init() { void HumanoidRobot<DOF>::init() {
motor_manager_ = MotorsInfo::getInstance()->getMotorManager(); motor_manager_ = MotorsInfo::getInstance()->getMotorManager();
@ -140,7 +59,7 @@ void HumanoidRobot<DOF>::init() {
std::vector<double>(7, 1.0)); std::vector<double>(7, 1.0));
ik_solver_ = std::make_shared<PinocchioDlsIKSolver>(urdf_path_, ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S", "PELVIS_S",
"R_WRIST_R_S", "R_WRIST_R_S",
"R_FINGER_TIP_FIXED"); "R_FINGER_TIP_FIXED");

28
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -112,7 +112,7 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01"); auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs); auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
@ -231,7 +231,7 @@ TEST(HumanoidRobotTest, moveLSCurveLocalRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01"); auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs); auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
@ -368,7 +368,7 @@ TEST(HumanoidRobotTest, moveLRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01"); auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs); auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
@ -469,7 +469,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01"); auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam"); auto camera = dmgr.getDevice<AbstractCamera>("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"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01"); auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam"); auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam");
@ -990,21 +990,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps."; EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps.";
} }
TEST(HumanoidRobotTest, EYouTest) {
auto& dmgr = DeviceManager::getInstance();
auto robot_abs = dmgr.getDevice<AbstractRobot>("ec_robot_01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
std::vector<JointPoint> 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));
}

24
cmvr-es/main.cpp
View File

@ -9,13 +9,27 @@
#include <glog/logging.h> #include <glog/logging.h>
#include "service/grpc/include/server_runner.h" #include "service/grpc/include/server_runner.h"
#include "utils/base/include/logger.h" #include "utils/base/include/logger.h"
#include "common/utils/config_helper/include/config_helper.h"
using namespace cmvr::service; using namespace cmvr::service;
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
cmvr::config::ServerConfig server_config; std::string config_path;
cmvr::ConfigHelper::getServerConfig(server_config); if(argc == 1) {
ServerRunner runner(server_config); char exe_path[PATH_MAX] = {0};
runner.start(); 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(); runner.join();
google::ShutdownGoogleLogging(); google::ShutdownGoogleLogging();
return 0; return 0;

1
cmvr-es/monitor/abstract_monitor.h
View File

@ -13,7 +13,6 @@
namespace cmvr::monitor{ namespace cmvr::monitor{
class AbstractMonitor { class AbstractMonitor {
public: public:
AbstractMonitor() = default;
explicit AbstractMonitor(const XmlNode &cfg){ explicit AbstractMonitor(const XmlNode &cfg){
cfg_ = cfg; cfg_ = cfg;
freq_ = cfg_.getAttrDefault("freq", 1); freq_ = cfg_.getAttrDefault("freq", 1);

6
cmvr-es/monitor/diskmonitor/include/disk_monitor.h
View File

@ -8,21 +8,17 @@
#include <condition_variable> #include <condition_variable>
#include "../../abstract_monitor.h" #include "../../abstract_monitor.h"
#include "cmvr/config/monitor_config/monitor_config.pb.h"
namespace cmvr::monitor { namespace cmvr::monitor {
class DiskMonitor final : public AbstractMonitor{ class DiskMonitor final : public AbstractMonitor{
public: public:
explicit DiskMonitor(const XmlNode& config); explicit DiskMonitor(const XmlNode& config);
explicit DiskMonitor(const cmvr::config::DiskMonitorConfig& config); // 新增proto构造函数
~DiskMonitor(); ~DiskMonitor();
protected: protected:
bool check_() override; bool check_() override;
bool execution_() override; bool execution_() override;
private: private:
void initFromProto(const cmvr::config::DiskMonitorConfig& config); // 新增初始化方法
std::unordered_map<std::string,int> folders_; std::unordered_map<std::string,int> folders_;
std::vector<std::string> dirsToClean_; // 需要清理的目录列表 std::vector<std::string> dirsToClean_; // 需要清理的目录列表
mutable std::mutex mutex_; // 保护共享资源的互斥锁 mutable std::mutex mutex_; // 保护共享资源的互斥锁
@ -31,4 +27,6 @@ namespace cmvr::monitor {
}; };
} }
#endif //FOLDER_MONITOR_H #endif //FOLDER_MONITOR_H

64
cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp
View File

@ -14,6 +14,7 @@ using namespace cmvr::monitor;
namespace fs = std::filesystem; namespace fs = std::filesystem;
using namespace std::chrono; using namespace std::chrono;
// 文件信息结构体 // 文件信息结构体
struct FileInfo { struct FileInfo {
std::string path; std::string path;
@ -37,40 +38,51 @@ bool deleteOldestFile(const std::string& directoryPath) {
struct dirent* entry; struct dirent* entry;
struct stat fileStat; struct stat fileStat;
// 遍历目录中的每个条目
while ((entry = readdir(dir)) != nullptr) { while ((entry = readdir(dir)) != nullptr) {
// 跳过 "." 和 ".."
if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") { if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") {
continue; continue;
} }
// 构建完整路径
std::string filePath = directoryPath + "/" + entry->d_name; std::string filePath = directoryPath + "/" + entry->d_name;
// 获取文件状态信息
if (stat(filePath.c_str(), &fileStat) == -1) { if (stat(filePath.c_str(), &fileStat) == -1) {
std::cerr << "Failed to get file status: " << filePath << std::endl; std::cerr << "Failed to get file status: " << filePath << std::endl;
continue; continue;
} }
// 只处理普通文件
if (S_ISREG(fileStat.st_mode)) { if (S_ISREG(fileStat.st_mode)) {
FileInfo fileInfo; FileInfo fileInfo;
fileInfo.path = filePath; fileInfo.path = filePath;
// 优先使用 st_birthtime (如果可用)，否则使用 st_ctime
#ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME #ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME
fileInfo.creationTime = fileStat.st_birthtime; fileInfo.creationTime = fileStat.st_birthtime;
#else #else
fileInfo.creationTime = fileStat.st_ctime; fileInfo.creationTime = fileStat.st_ctime;
#endif #endif
files.push_back(fileInfo); files.push_back(fileInfo);
} }
} }
closedir(dir); closedir(dir);
// 如果没有找到文件，返回失败
if (files.empty()) { if (files.empty()) {
std::cout << "No files found in directory: " << directoryPath << std::endl; std::cout << "No files found in directory: " << directoryPath << std::endl;
return false; return false;
} }
// 按创建时间排序
std::sort(files.begin(), files.end(), compareByCreationTime); 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) { if (unlink(oldestFilePath.c_str()) == -1) {
std::cerr << "Failed to delete file: " << oldestFilePath << std::endl; std::cerr << "Failed to delete file: " << oldestFilePath << std::endl;
return false; return false;
@ -80,11 +92,11 @@ bool deleteOldestFile(const std::string& directoryPath) {
return true; return true;
} }
bool pathExists(const std::string& path) { bool pathExists(const std::string& path) {
struct stat buffer; struct stat buffer;
return (stat(path.c_str(), &buffer) == 0); return (stat(path.c_str(), &buffer) == 0);
} }
int countFilesInDirectory(const std::string& path) { int countFilesInDirectory(const std::string& path) {
DIR* dir = opendir(path.c_str()); DIR* dir = opendir(path.c_str());
if (!dir) { if (!dir) {
@ -95,7 +107,9 @@ int countFilesInDirectory(const std::string& path) {
int count = 0; int count = 0;
struct dirent* entry; struct dirent* entry;
// 遍历目录中的每个条目
while ((entry = readdir(dir)) != nullptr) { while ((entry = readdir(dir)) != nullptr) {
// 跳过 "." 和 ".."
if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") { if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") {
count++; count++;
} }
@ -105,8 +119,8 @@ int countFilesInDirectory(const std::string& path) {
return count; return count;
} }
// XML构造函数
DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){
try { try {
for (auto &node: config.getChildren()){ for (auto &node: config.getChildren()){
string fileDir = node.getAttrString("fileDir"); 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())); 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() { DiskMonitor::~DiskMonitor() {
folders_.clear(); folders_.clear();
} }
bool DiskMonitor::check_() { bool DiskMonitor::check_() {
// 检查当前目录下的文件数量是否超过最大值
bool allOk = true; bool allOk = true;
// 遍历所有监控的文件夹
for (const auto& folder : folders_) { for (const auto& folder : folders_) {
const std::string& path = folder.first; const std::string& path = folder.first;
size_t maxFiles = folder.second; size_t maxFiles = folder.second;
@ -169,21 +154,28 @@ bool DiskMonitor::check_() {
continue; continue;
} }
int nFileCount = countFilesInDirectory(path); int nFileCount = countFilesInDirectory(path);
if (nFileCount > maxFiles) { if (nFileCount > maxFiles)
{
allOk = false; allOk = false;
dirsToClean_.push_back(path); dirsToClean_.push_back(path);
std::cout << "path need remove oldest file: " << path << std::endl; std::cout << "path need remove oldest file: " << path << std::endl;
} }
} catch (const fs::filesystem_error& e) { } catch (const fs::filesystem_error& e) {
std::cerr << "Filesystem error: " << e.what() << '\n'; std::cerr << "Filesystem error: " << e.what() << '\n';
} catch (const std::exception& e) { } catch (const std::exception& e) {
std::cerr << "Unexpected error: " << e.what() << '\n'; std::cerr << "Unexpected error: " << e.what() << '\n';
} }
} }
return allOk; return allOk;
} }
bool DiskMonitor::execution_() { bool DiskMonitor::execution_() {
// 删除创建时间最早的文件
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
bool success = true; bool success = true;
@ -191,6 +183,6 @@ bool DiskMonitor::execution_() {
deleteOldestFile(path); deleteOldestFile(path);
} }
dirsToClean_.clear(); dirsToClean_.clear(); // 清理完成后清空列表
return success; return success;
} }

15
cmvr-es/monitor_manager/include/monitor_factory.h
View File

@ -5,8 +5,8 @@
#ifndef MONITOR_FACTORY_H #ifndef MONITOR_FACTORY_H
#define 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 namespace cmvr::monitor
{ {
@ -14,17 +14,14 @@ namespace cmvr::monitor
{ {
public: public:
MonitorFactory() = default; MonitorFactory() = default;
// 原有的XML方式
template <typename MonitorType> template <typename MonitorType>
std::shared_ptr<MonitorType> create(const XmlNode& cfg); std::shared_ptr<MonitorType> create(const XmlNode& cfg);
std::shared_ptr<AbstractMonitor> create(const XmlNode& cfg); std::shared_ptr<AbstractMonitor> create(const XmlNode& cfg);
// 新增proto方式
template <typename MonitorType>
std::shared_ptr<MonitorType> create(const cmvr::config::DiskMonitorConfig& config);
std::shared_ptr<AbstractMonitor> create(const cmvr::config::DiskMonitorConfig& config);
}; };
} }
#endif //MONITOR_FACTORY_H #endif //MONITOR_FACTORY_H

23
cmvr-es/monitor_manager/include/monitor_manager.h
View File

@ -15,38 +15,23 @@ namespace cmvr::monitor {
public: public:
MonitorManager(const MonitorManager&) = delete; MonitorManager(const MonitorManager&) = delete;
MonitorManager& operator=(const MonitorManager&) = delete; MonitorManager& operator=(const MonitorManager&) = delete;
// 原有的XML方式
static MonitorManager& getInstance(const XmlNode &cfg); static MonitorManager& getInstance(const XmlNode &cfg);
// 新增proto方式
static MonitorManager& getInstance(const cmvr::config::MonitorManagerConfig& config);
static MonitorManager& getInstance(); static MonitorManager& getInstance();
static void destroyInstance(); static void destroyInstance();
private: private:
// 原有的XML方式
explicit MonitorManager (const XmlNode &cfg); explicit MonitorManager (const XmlNode &cfg);
void init_monitors();
// 新增proto方式
explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config);
void init_monitors(); // XML方式
void init_monitors_from_proto(); // Proto方式
private: private:
static std::once_flag init_flag_; static std::once_flag init_flag_;
static std::shared_ptr<MonitorManager> instance_; static std::shared_ptr<MonitorManager> instance_;
// 两种配置方式，只有一个有效 XmlNode cfg_;
bool use_proto_ = false;
XmlNode cfg_; // XML配置
cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置
std::shared_ptr<MonitorFactory> monitor_factory_; std::shared_ptr<MonitorFactory> monitor_factory_;
std::unordered_map<std::string, std::shared_ptr<AbstractMonitor>> monitors_; std::unordered_map<std::string, std::shared_ptr<AbstractMonitor>> monitors_;
}; };
} }
#endif //MONITOR_MANAGER_H #endif //MONITOR_MANAGER_H

28
cmvr-es/monitor_manager/src/monitor_factory.cpp
View File

@ -3,12 +3,12 @@
// //
#include "../include/monitor_factory.h" #include "../include/monitor_factory.h"
#include "../../monitor/diskmonitor/include/disk_monitor.h" #include "../../monitor/diskmonitor/include/disk_monitor.h"
using namespace std; using namespace std;
using namespace cmvr::monitor; using namespace cmvr::monitor;
// XML方式的模板特化
template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const XmlNode&); template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const XmlNode&);
template <typename MonitorType> template <typename MonitorType>
@ -17,30 +17,8 @@ std::shared_ptr<MonitorType> MonitorFactory::create(const XmlNode& cfg) {
return std::make_shared<DiskMonitor>(cfg); return std::make_shared<DiskMonitor>(cfg);
} }
else { else {
LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[MonitorFactory]: Unsupported monitor type"); throw runtime_error("[DeviceFactory]: Unsupported device type");
} }
} }
// Proto方式的模板特化
template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const cmvr::config::DiskMonitorConfig&);
template <typename MonitorType>
std::shared_ptr<MonitorType> MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) {
if constexpr (std::is_same_v<MonitorType, DiskMonitor>) {
return std::make_shared<DiskMonitor>(config);
}
else {
LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type";
throw runtime_error("[MonitorFactory]: Unsupported monitor type");
}
}
std::shared_ptr<AbstractMonitor> MonitorFactory::create(const XmlNode& cfg) {
// 保留原有实现
return nullptr;
}
std::shared_ptr<AbstractMonitor> MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) {
return std::make_shared<DiskMonitor>(config);
}

54
cmvr-es/monitor_manager/src/monitor_manager.cpp
View File

@ -3,7 +3,8 @@
// //
#include "../include/monitor_manager.h" #include "../include/monitor_manager.h"
#include "monitor/diskmonitor/include/disk_monitor.h"
#include "../../monitor/diskmonitor/include/disk_monitor.h"
using namespace std; using namespace std;
using namespace cmvr::monitor; using namespace cmvr::monitor;
@ -11,19 +12,11 @@ using namespace cmvr::monitor;
std::shared_ptr<MonitorManager> MonitorManager::instance_ = nullptr; std::shared_ptr<MonitorManager> MonitorManager::instance_ = nullptr;
std::once_flag MonitorManager::init_flag_; std::once_flag MonitorManager::init_flag_;
// XML方式构造函数 MonitorManager::MonitorManager(const XmlNode& cfg) {
MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) {
monitor_factory_ = make_shared<MonitorFactory>(); monitor_factory_ = make_shared<MonitorFactory>();
cfg_ = cfg;
init_monitors(); init_monitors();
} }
// Proto方式构造函数
MonitorManager::MonitorManager(const cmvr::config::MonitorManagerConfig& config) : use_proto_(true), proto_cfg_(config) {
monitor_factory_ = make_shared<MonitorFactory>();
init_monitors_from_proto();
}
// XML方式单例
MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) {
std::call_once(init_flag_, [&cfg] { std::call_once(init_flag_, [&cfg] {
instance_.reset(new MonitorManager(cfg)); instance_.reset(new MonitorManager(cfg));
@ -31,17 +24,9 @@ MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) {
return *instance_; 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() { MonitorManager& MonitorManager::getInstance() {
if (!instance_) { 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_; return *instance_;
} }
@ -50,14 +35,13 @@ void MonitorManager::destroyInstance() {
instance_.reset(); instance_.reset();
} }
// XML方式初始化
void MonitorManager::init_monitors() { void MonitorManager::init_monitors() {
try{ try{
auto diskmonitor_node = cfg_.getChild("DiskMonitor"); auto diskmonitor_node = cfg_.getChild("DiskMonitor");
auto id = diskmonitor_node.getAttrString("id"); auto id = diskmonitor_node.getAttrString("id");
auto monitor = monitor_factory_->create<DiskMonitor>(diskmonitor_node); auto monitor = monitor_factory_->create<DiskMonitor>(diskmonitor_node);
if (monitor == nullptr) { 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; monitors_[id] = monitor;
} }
@ -65,31 +49,5 @@ void MonitorManager::init_monitors() {
LOG(ERROR) << e.what(); LOG(ERROR) << e.what();
throw runtime_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<DiskMonitor>(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());
}
} }

7
cmvr-es/service/grpc/include/server_runner.h
View File

@ -11,14 +11,13 @@
#include <thread> #include <thread>
#include <grpcpp/grpcpp.h> #include <grpcpp/grpcpp.h>
#include "rapidxml/xml_parser.h" #include "rapidxml/xml_parser.h"
#include "cmvr/config/server_config/server_config.pb.h"
namespace cmvr::service { namespace cmvr::service {
class ServerRunner { class ServerRunner {
public: public:
ServerRunner(); ServerRunner();
ServerRunner(const cmvr::config::ServerConfig& config); explicit ServerRunner(XmlNode cfg);
~ServerRunner(); ~ServerRunner();
ServerRunner(const ServerRunner&) = delete; ServerRunner(const ServerRunner&) = delete;
@ -26,7 +25,7 @@ namespace cmvr::service {
ServerRunner(ServerRunner&&) = delete; ServerRunner(ServerRunner&&) = delete;
ServerRunner& operator=(ServerRunner&&) = delete; ServerRunner& operator=(ServerRunner&&) = delete;
void start(); void start(XmlNode cfg);
void stop(); void stop();
void join(); void join();
@ -41,7 +40,7 @@ namespace cmvr::service {
std::condition_variable cv_; std::condition_variable cv_;
std::thread worker_; std::thread worker_;
cmvr::config::ServerConfig cfg_; XmlNode cfg_;
std::unique_ptr<grpc::Server> server_; std::unique_ptr<grpc::Server> server_;
std::string address_; std::string address_;

2
cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
View File

@ -17,7 +17,7 @@ using google::protobuf::util::TimeUtil;
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()) gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance())
{ {
pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
,"PELVIS_S" ,"PELVIS_S"
,"R_WRIST_R_S"); ,"R_WRIST_R_S");
pinocchio_ik_->init(); pinocchio_ik_->init();

26
cmvr-es/service/grpc/src/server_runner.cpp
View File

@ -25,21 +25,23 @@
using namespace cmvr::service; using namespace cmvr::service;
ServerRunner::ServerRunner() = default; ServerRunner::ServerRunner() = default;
ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config) ServerRunner::ServerRunner(XmlNode cfg) {
{ start(std::move(cfg));
// start();
} }
ServerRunner::~ServerRunner() { ServerRunner::~ServerRunner() {
stop(); stop();
join(); join();
} }
void ServerRunner::start() { void ServerRunner::start(XmlNode cfg) {
std::unique_lock<std::mutex> lk(mtx_); std::unique_lock<std::mutex> lk(mtx_);
if (worker_.joinable()) { if (worker_.joinable()) {
throw std::runtime_error("ServerRunner already started"); throw std::runtime_error("ServerRunner already started");
} }
cfg_ = std::move(cfg);
stop_requested_ = false; stop_requested_ = false;
started_ = false; started_ = false;
start_failed_ = false; start_failed_ = false;
@ -95,15 +97,25 @@ void ServerRunner::threadMain() {
using namespace cmvr::monitor; using namespace cmvr::monitor;
try { try {
if (!cfg_.hasChild("DeviceManager")) {
throw std::runtime_error("Device Manager node not found");
}
static std::once_flag reflection_once; static std::once_flag reflection_once;
std::call_once(reflection_once, []() { std::call_once(reflection_once, []() {
grpc::reflection::InitProtoReflectionServerBuilderPlugin(); 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; std::string local_address = "0.0.0.0:" + port;
auto camera_service = std::make_unique<gRPCCameraServiceImpl>(); auto camera_service = std::make_unique<gRPCCameraServiceImpl>();

44
protos/cmvr/config/biohead_config/biohead_config.proto
View File

@ -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;
}

22
protos/cmvr/config/monitor_config/monitor_config.proto
View File

@ -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; // 磁盘监控配置
}

88
protos/cmvr/config/robot_config/robot_config.proto
View File

@ -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机器人
}

10
protos/cmvr/config/server_config/server_config.proto
View File

@ -1,10 +0,0 @@
syntax = "proto3";
package cmvr.config;
message GrpcServerConfig{
string port = 1;
}
message ServerConfig{
GrpcServerConfig grpc_server = 1;
}

1
request.txt
View File

@ -33,6 +33,5 @@ third_party/modbus/3.1.11
third_party/visp/3.7.0 third_party/visp/3.7.0
third_party/mainif/0.0.5 third_party/mainif/0.0.5
third_party/matplotplusplus/1.2.0 third_party/matplotplusplus/1.2.0
third_party/eyou_ethercat