update proto config
This commit is contained in:
parent
b050942838
commit
9fb185b5a8
@ -21,7 +21,7 @@ void run_touch_once(int u, int v) {
|
||||
|
||||
const XmlNode config(kConfigPath);
|
||||
// auto& dm = cmvr::device::DeviceManager::getInstance();
|
||||
cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
|
||||
cmvr::device::DeviceManager::getInstance();
|
||||
|
||||
|
||||
// cmvr::service::ServerRunner runner;
|
||||
|
||||
27
cmvr-es/common/config/biohead_config/biohead_config.pb.txt
Normal file
27
cmvr-es/common/config/biohead_config/biohead_config.pb.txt
Normal file
@ -0,0 +1,27 @@
|
||||
bioheads {
|
||||
id: "bio_head"
|
||||
serial: "/dev/ttyUSB0"
|
||||
ctrlFreq: 50
|
||||
enable: false
|
||||
|
||||
eyebrow {
|
||||
serial: "64:0~3"
|
||||
offest: "90 90 90 90"
|
||||
jLmtUp: "90 170 155 110"
|
||||
jLmtLow: "20 77 90 20"
|
||||
}
|
||||
|
||||
eye {
|
||||
serial: "64:4~9"
|
||||
offest: "90 90 90 90 90 90"
|
||||
jLmtUp: "90 150 165 90 120 115"
|
||||
jLmtLow: "20 90 90 25 70 75"
|
||||
}
|
||||
|
||||
mouth {
|
||||
serial: "65:0~9"
|
||||
offest: "90 90 90 90 90 90 90 90 90 90"
|
||||
jLmtUp: "150 110 130 140 100 105 110 125 90 95"
|
||||
jLmtLow: "70 30 80 80 65 55 45 80 85 90"
|
||||
}
|
||||
}
|
||||
@ -1,152 +0,0 @@
|
||||
<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>
|
||||
@ -27,7 +27,7 @@ realsense_cameras {
|
||||
align_mode: ALIGN_MODE_COLOR
|
||||
buffer_size: 30
|
||||
sync: false
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
realsense_cameras {
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
rh56dftp_dexhands {
|
||||
id: "hand1"
|
||||
ip: "192.168.1.213"
|
||||
ip: "192.168.1.223"
|
||||
port: 6000
|
||||
poll_interval_ms: 10
|
||||
enable: false
|
||||
@ -11,7 +11,7 @@ rh56dftp_dexhands {
|
||||
ip: "192.168.1.224"
|
||||
port: 6000
|
||||
poll_interval_ms: 10
|
||||
enable: true
|
||||
enable: false
|
||||
}
|
||||
|
||||
px_6ax_gen3 {
|
||||
|
||||
25
cmvr-es/common/config/monitor_config/monitor_config.pb.txt
Normal file
25
cmvr-es/common/config/monitor_config/monitor_config.pb.txt
Normal file
@ -0,0 +1,25 @@
|
||||
# 监控管理器配置
|
||||
monitor_manager {
|
||||
disk_monitor {
|
||||
id: "file_monitor"
|
||||
freq: 1
|
||||
enable: false
|
||||
|
||||
folders {
|
||||
fileDir: "/home/share/assets/audio"
|
||||
maxVolume: 1000
|
||||
}
|
||||
folders {
|
||||
fileDir: "/home/share/assets/image"
|
||||
maxVolume: 1000
|
||||
}
|
||||
folders {
|
||||
fileDir: "/home/share/assets/video"
|
||||
maxVolume: 1000
|
||||
}
|
||||
folders {
|
||||
fileDir: "../log"
|
||||
maxVolume: 1000
|
||||
}
|
||||
}
|
||||
}
|
||||
212
cmvr-es/common/config/robot_config/robot_config.pb.txt
Normal file
212
cmvr-es/common/config/robot_config/robot_config.pb.txt
Normal file
@ -0,0 +1,212 @@
|
||||
# HumanRobot配置
|
||||
humanrobots {
|
||||
id: "hc01"
|
||||
dof: 14
|
||||
urdf: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
|
||||
baseLink: "PELVIS_S"
|
||||
jointNames: "L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
|
||||
linkNames: "PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
|
||||
bufferSize: 50
|
||||
verbose: false
|
||||
enable: false
|
||||
|
||||
can_manager {
|
||||
id: ""
|
||||
devId: ""
|
||||
|
||||
left_arm {
|
||||
id: ""
|
||||
devId: ""
|
||||
channelId: 0
|
||||
enable: false
|
||||
toolFrame: "L_FINGER_TIP"
|
||||
motors {
|
||||
id: 23
|
||||
jointName: "L_SHOULDER_P"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 24
|
||||
jointName: "L_SHOULDER_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 25
|
||||
jointName: "L_SHOULDER_Y"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 26
|
||||
jointName: "L_ELBOW_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 27
|
||||
jointName: "L_WRIST_P"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 28
|
||||
jointName: "L_WRIST_Y"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 29
|
||||
jointName: "L_WRIST_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
}
|
||||
|
||||
right_arm {
|
||||
id: ""
|
||||
devId: ""
|
||||
channelId: 1
|
||||
enable: true
|
||||
toolFrame: "R_FINGER_TIP"
|
||||
motors {
|
||||
id: 16
|
||||
jointName: "R_SHOULDER_P"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 17
|
||||
jointName: "R_SHOULDER_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 18
|
||||
jointName: "R_SHOULDER_Y"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 19
|
||||
jointName: "R_ELBOW_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 20
|
||||
jointName: "R_WRIST_P"
|
||||
limitQLb: -3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 21
|
||||
jointName: "R_WRIST_Y"
|
||||
limitQLb: -1.102
|
||||
limitQUb: 1.02
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 22
|
||||
jointName: "R_WRIST_R"
|
||||
limitQLb: -0.293
|
||||
limitQUb: 1.57079
|
||||
limitQd: 3.0
|
||||
}
|
||||
}
|
||||
|
||||
head {
|
||||
id: ""
|
||||
devId: ""
|
||||
channelId: 2
|
||||
enable: true
|
||||
motors {
|
||||
id: 32
|
||||
jointName: "HEAD_Y"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 30
|
||||
jointName: "HEAD_P"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 31
|
||||
jointName: "HEAD_R"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
}
|
||||
|
||||
waist {
|
||||
id: ""
|
||||
devId: ""
|
||||
channelId: 3
|
||||
enable: false
|
||||
motors {
|
||||
id: 4
|
||||
jointName: "WAIST_Y"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
motors {
|
||||
id: 15
|
||||
jointName: "WAIST_P"
|
||||
limitQLb: 3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
# EtherCAT机器人配置
|
||||
ethercat_robots {
|
||||
id: "ec_robot_01"
|
||||
dof: 6
|
||||
urdf: "/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
|
||||
baseLink: "PELVIS_S"
|
||||
jointNames: "L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
|
||||
linkNames: "PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
|
||||
bufferSize: 100
|
||||
verbose: true
|
||||
updFreq: 1000
|
||||
enable: true
|
||||
|
||||
# EtherCAT主站配置
|
||||
master {
|
||||
interface: "enp1s0"
|
||||
cycle_time_us: 1000
|
||||
enable: true
|
||||
}
|
||||
|
||||
# 电机配置列表
|
||||
motors {
|
||||
slave: 1
|
||||
jointName: "L_SHOULDER_P"
|
||||
limitQLb: -3.14
|
||||
limitQUb: 3.14
|
||||
limitQd: 3.0
|
||||
}
|
||||
}
|
||||
3
cmvr-es/common/config/server_config/server_config.pb.txt
Normal file
3
cmvr-es/common/config/server_config/server_config.pb.txt
Normal file
@ -0,0 +1,3 @@
|
||||
grpc_server {
|
||||
port: "50052"
|
||||
}
|
||||
@ -10,6 +10,9 @@
|
||||
#include "cmvr/config/microphone_config/microphone_config.pb.h"
|
||||
#include "cmvr/config/speaker_config/speaker_conifg.pb.h"
|
||||
#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
|
||||
#include "cmvr/config/biohead_config/biohead_config.pb.h"
|
||||
#include "cmvr/config/robot_config/robot_config.pb.h"
|
||||
#include "cmvr/config/server_config/server_config.pb.h"
|
||||
|
||||
#define GET_CONFIG(file, para) \
|
||||
([&]() -> bool { \
|
||||
@ -82,6 +85,20 @@ namespace cmvr
|
||||
return SET_CONFIG(config, touch_screen_app_config_file);
|
||||
}
|
||||
|
||||
static bool getHeadConfig(config::HeadConfig& config)
|
||||
{
|
||||
return GET_CONFIG(biohead_config_file, config);
|
||||
}
|
||||
|
||||
static bool getRobotConfig(config::RobotConfig& config)
|
||||
{
|
||||
return GET_CONFIG(robot_config_file, config);
|
||||
}
|
||||
|
||||
static bool getServerConfig(config::ServerConfig& config)
|
||||
{
|
||||
return GET_CONFIG(server_config_file, config);
|
||||
}
|
||||
private:
|
||||
// Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx)
|
||||
template <class T>
|
||||
|
||||
@ -6,3 +6,6 @@ DECLARE_string(dexhand_config_file);
|
||||
DECLARE_string(microphone_config_file);
|
||||
DECLARE_string(speaker_config_file);
|
||||
DECLARE_string(touch_screen_app_config_file);
|
||||
DECLARE_string(biohead_config_file);
|
||||
DECLARE_string(robot_config_file);
|
||||
DECLARE_string(server_config_file);
|
||||
|
||||
@ -88,3 +88,15 @@ DEFINE_string(speaker_config_file,
|
||||
DEFINE_string(touch_screen_app_config_file,
|
||||
basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
|
||||
"The configuration file for TouchScreenApp");
|
||||
|
||||
DEFINE_string(biohead_config_file,
|
||||
basePath() + "biohead_config/biohead_config.pb.txt",
|
||||
"The configuration file for BioHead");
|
||||
|
||||
DEFINE_string(robot_config_file,
|
||||
basePath() + "robot_config/robot_config.pb.txt",
|
||||
"The configuration file for Robot");
|
||||
|
||||
DEFINE_string(server_config_file,
|
||||
basePath() + "server_config/server_config.pb.txt",
|
||||
"The configuration file for Server");
|
||||
|
||||
@ -11,6 +11,7 @@ target_link_libraries(data_center PRIVATE
|
||||
cmvr_es::utils
|
||||
cmvr_es::device::canbus
|
||||
cmvr_es::device::ti5motor
|
||||
cmvr_es::device::eyoumotor
|
||||
protobuf
|
||||
glog
|
||||
)
|
||||
|
||||
@ -23,8 +23,8 @@ namespace cmvr {
|
||||
|
||||
public:
|
||||
~MotorsInfo(){};
|
||||
|
||||
void init(const XmlNode &cfg);
|
||||
void init(const cmvr::config::HumanRobotConfig& config);
|
||||
void init(const cmvr::config::EthercatRobotConfig& config);
|
||||
|
||||
static MotorsInfo *getInstance() {
|
||||
static MotorsInfo instance;
|
||||
|
||||
@ -3,6 +3,8 @@
|
||||
//
|
||||
|
||||
#include "data_center/include/motors_info.h"
|
||||
|
||||
#include "motor/eyou_motor/eyou_motor.h"
|
||||
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||||
#include "motor/ti5_motor/ti5_motor.h"
|
||||
|
||||
@ -12,99 +14,95 @@ using namespace cmvr::device;
|
||||
MotorsInfo::MotorsInfo() {
|
||||
}
|
||||
|
||||
void MotorsInfo::init(const XmlNode &cfg) {
|
||||
// 定义 lambda 函数，用于读取 XML 节点 enable 属性
|
||||
auto readEnable = [](const XmlNode &node) -> bool {
|
||||
std::string enable_str = node.getAttrString("enable"); // 默认 false
|
||||
return (enable_str == "true" || enable_str == "1");
|
||||
};
|
||||
auto can_cfg = cfg.getChild("CanManger");
|
||||
void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) {
|
||||
const auto& can_manager = config.can_manager();
|
||||
|
||||
auto l_can_cfg = can_cfg.getChild("LeftArmCan");
|
||||
left_arm_enabled_ = readEnable(l_can_cfg);
|
||||
// 读取enable状态
|
||||
left_arm_enabled_ = can_manager.left_arm().enable();
|
||||
right_arm_enabled_ = can_manager.right_arm().enable();
|
||||
waist_enabled_ = can_manager.waist().enable();
|
||||
head_enabled_ = can_manager.head().enable();
|
||||
|
||||
// 局部变量存储电机配置
|
||||
std::vector<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_) {
|
||||
l_motors_cfg_ = l_can_cfg.getChildren("Motor");
|
||||
l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
|
||||
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||||
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||||
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||||
const auto& left_arm = can_manager.left_arm();
|
||||
for (const auto& motor : left_arm.motors()) {
|
||||
l_motors_cfg.push_back(motor);
|
||||
}
|
||||
l_can_client_ = std::make_shared<SocketCanClientRaw>(left_arm);
|
||||
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
|
||||
l_can_receiver_ = std::make_shared<CanReceiver<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_) {
|
||||
r_motors_cfg_ = r_can_cfg.getChildren("Motor");
|
||||
r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
|
||||
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||||
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||||
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||||
const auto& right_arm = can_manager.right_arm();
|
||||
for (const auto& motor : right_arm.motors()) {
|
||||
r_motors_cfg.push_back(motor);
|
||||
}
|
||||
r_can_client_ = std::make_shared<SocketCanClientRaw>(right_arm);
|
||||
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
|
||||
r_can_receiver_ = std::make_shared<CanReceiver<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_) {
|
||||
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
|
||||
waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
|
||||
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||||
waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||||
waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||||
const auto& waist = can_manager.waist();
|
||||
for (const auto& motor : waist.motors()) {
|
||||
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_receiver_ = std::make_shared<CanReceiver<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_) {
|
||||
head_motors_cfg_ = head_can_cfg.getChildren("Motor");
|
||||
head_can_client_ = std::make_shared<SocketCanClientRaw>(head_can_cfg);
|
||||
head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||||
head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||||
head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||||
const auto& head = can_manager.head();
|
||||
for (const auto& motor : head.motors()) {
|
||||
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_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
|
||||
head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
|
||||
}
|
||||
|
||||
// 开始初始化
|
||||
// 初始化 limbs
|
||||
struct Limb {
|
||||
std::string name;
|
||||
bool enabled;
|
||||
std::shared_ptr<AbstractCanbus> client;
|
||||
std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
|
||||
std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
|
||||
std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
|
||||
std::vector<XmlNode> motor_cfgs;
|
||||
std::shared_ptr<CanSender<msgs::RobotDetail>> sender;
|
||||
std::shared_ptr<CanReceiver<msgs::RobotDetail>> receiver;
|
||||
std::shared_ptr<MessageManager<msgs::RobotDetail>> message_manager;
|
||||
std::vector<cmvr::config::MotorConfig> motor_cfgs;
|
||||
};
|
||||
|
||||
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_
|
||||
},
|
||||
{
|
||||
"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_,
|
||||
r_motors_cfg_
|
||||
},
|
||||
{
|
||||
"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_,
|
||||
head_motors_cfg_
|
||||
}
|
||||
{"WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, waist_motors_cfg},
|
||||
{"LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, l_motors_cfg},
|
||||
{"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, r_motors_cfg},
|
||||
{"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, head_motors_cfg}
|
||||
};
|
||||
|
||||
// 创建 MotorManager
|
||||
motor_manager_ = std::make_shared<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.client) limb.client->init();
|
||||
|
||||
// 2. 初始化 Sender / Receiver（如果有）
|
||||
if (limb.sender && limb.receiver && limb.client) {
|
||||
auto ret = limb.sender->Init(limb.client.get(), false);
|
||||
if (ret != ErrorCode::OK)
|
||||
@ -124,19 +122,16 @@ void MotorsInfo::init(const XmlNode &cfg) {
|
||||
LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
|
||||
}
|
||||
|
||||
// 3. 创建协议（如果有CAN）
|
||||
std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
|
||||
if (limb.sender && limb.message_manager) {
|
||||
protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
|
||||
}
|
||||
|
||||
// 4. 并行初始化电机
|
||||
if (!limb.motor_cfgs.empty()) {
|
||||
tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
|
||||
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name <<
|
||||
" motors...";
|
||||
for (const auto &cfg: limb.motor_cfgs) {
|
||||
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||||
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << " motors...";
|
||||
for (const auto& motor_cfg : limb.motor_cfgs) {
|
||||
auto motor = std::make_shared<Ti5Motor>(motor_cfg);
|
||||
if (protocol) motor->setProtocol(protocol);
|
||||
motor->init();
|
||||
motor_manager_->addMotor(motor);
|
||||
@ -145,12 +140,26 @@ void MotorsInfo::init(const XmlNode &cfg) {
|
||||
}
|
||||
}
|
||||
|
||||
// 等待所有任务完成
|
||||
for (auto &task: tasks) task.get();
|
||||
|
||||
for (auto& task : tasks) task.get();
|
||||
LOG(INFO) << "All enabled motors initialized successfully.";
|
||||
}
|
||||
|
||||
|
||||
void MotorsInfo::init(const cmvr::config::EthercatRobotConfig& config)
|
||||
{
|
||||
if (!motor_manager_)
|
||||
{
|
||||
motor_manager_ = std::make_shared<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,
|
||||
std::unordered_map<std::string, double> &joint_qs) const {
|
||||
auto res{true};
|
||||
|
||||
@ -28,15 +28,7 @@ namespace cmvr::device {
|
||||
template <typename DeviceType, typename ConfigType>
|
||||
std::shared_ptr<DeviceType> createFromConfig(const ConfigType& cfg);
|
||||
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);
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -41,7 +41,6 @@ namespace cmvr::device {
|
||||
DeviceManager(const DeviceManager&) = delete;
|
||||
DeviceManager& operator=(const DeviceManager&) = delete;
|
||||
|
||||
static DeviceManager& getInstance(const XmlNode &cfg);
|
||||
static DeviceManager& getInstance();
|
||||
static void destroyInstance();
|
||||
|
||||
@ -68,7 +67,7 @@ namespace cmvr::device {
|
||||
std::unique_ptr<DeviceFactory> dev_factory_;
|
||||
std::unique_ptr<device::SystemMonitor> sys_monitor_;
|
||||
|
||||
explicit DeviceManager(const XmlNode &cfg);
|
||||
explicit DeviceManager();
|
||||
void init_devices_();
|
||||
void get_os_info_();
|
||||
};
|
||||
|
||||
@ -18,172 +18,16 @@
|
||||
//#include "devices/robot/aubo_robot/include/aubo_robot.h"
|
||||
using namespace std;
|
||||
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::RealSenseCameraConfig& 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<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& 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>
|
||||
std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cfg)
|
||||
{
|
||||
@ -244,6 +88,38 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
|
||||
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 {
|
||||
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
|
||||
throw runtime_error("[DeviceFactory]: Unsupported device type");
|
||||
@ -253,3 +129,4 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -24,12 +24,9 @@ std::shared_ptr<DeviceManager> DeviceManager::instance_ = nullptr;
|
||||
std::once_flag DeviceManager::init_flag_;
|
||||
|
||||
|
||||
DeviceManager::DeviceManager(const XmlNode& cfg) {
|
||||
DeviceManager::DeviceManager() {
|
||||
try {
|
||||
cfg_ = cfg;
|
||||
info_.version = cfg_.getAttrDefault("ver", "1.0");
|
||||
info_.name = cfg_.getAttrDefault("name", "cmvr_es");
|
||||
info_.description = cfg_.getAttrDefault("description", "empty");
|
||||
|
||||
get_os_info_();
|
||||
|
||||
dev_factory_ = std::make_unique<DeviceFactory>();
|
||||
@ -42,17 +39,12 @@ DeviceManager::DeviceManager(const XmlNode& cfg) {
|
||||
}
|
||||
}
|
||||
|
||||
DeviceManager& DeviceManager::getInstance(const XmlNode& cfg) {
|
||||
std::call_once(init_flag_, [&cfg] {
|
||||
instance_.reset(new DeviceManager(cfg));
|
||||
});
|
||||
return *instance_;
|
||||
}
|
||||
|
||||
DeviceManager& DeviceManager::getInstance() {
|
||||
if (!instance_) {
|
||||
throw std::runtime_error("[DeviceManager] (getInstance): DeviceManager not initialized. Call getInstance(const XmlNode&) first.");
|
||||
}
|
||||
if (!instance_)
|
||||
std::call_once(init_flag_, [&] {
|
||||
instance_.reset(new DeviceManager());
|
||||
});
|
||||
|
||||
return *instance_;
|
||||
}
|
||||
|
||||
@ -199,38 +191,38 @@ void DeviceManager::getSystemStatus(SystemStatus& status) const {
|
||||
|
||||
void DeviceManager::init_devices_() {
|
||||
try{
|
||||
auto dmgr_node = cfg_.getChild("Devices");
|
||||
auto battery_node = dmgr_node.getChild("Battery");
|
||||
for (auto &node: battery_node.getChildren()){
|
||||
string id = node.getAttrString("id");
|
||||
if (devices_.count(id)) {
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id);
|
||||
}
|
||||
auto device = dev_factory_->create<AbstractBattery>(node);
|
||||
if (device == nullptr) {
|
||||
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
}
|
||||
devices_[id] = device;
|
||||
std::get<std::shared_ptr<AbstractBattery>>(devices_[id])->init();
|
||||
LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success";
|
||||
}
|
||||
// auto dmgr_node = cfg_.getChild("Devices");
|
||||
// auto battery_node = dmgr_node.getChild("Battery");
|
||||
// for (auto &node: battery_node.getChildren()){
|
||||
// string id = node.getAttrString("id");
|
||||
// if (devices_.count(id)) {
|
||||
// LOG(ERROR) << "[DeviceManager]: Duplicate Battery Device ID " << id;
|
||||
// throw runtime_error("[DeviceManager]: Duplicate Battery Device ID " + id);
|
||||
// }
|
||||
// auto device = dev_factory_->create<AbstractBattery>(node);
|
||||
// if (device == nullptr) {
|
||||
// throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
// }
|
||||
// devices_[id] = device;
|
||||
// std::get<std::shared_ptr<AbstractBattery>>(devices_[id])->init();
|
||||
// LOG(INFO) << "[DeviceManager]: Init Battery " << id << " Success";
|
||||
// }
|
||||
|
||||
auto agv_node = dmgr_node.getChild("AGV");
|
||||
for (auto &node: agv_node.getChildren()){
|
||||
string id = node.getAttrString("id");
|
||||
if (devices_.count(id)) {
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id);
|
||||
}
|
||||
auto device = dev_factory_->create<AbstractAGV>(node);
|
||||
if (device == nullptr) {
|
||||
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
}
|
||||
devices_[id] = device;
|
||||
std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init();
|
||||
LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success";
|
||||
}
|
||||
// auto agv_node = dmgr_node.getChild("AGV");
|
||||
// for (auto &node: agv_node.getChildren()){
|
||||
// string id = node.getAttrString("id");
|
||||
// if (devices_.count(id)) {
|
||||
// LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id;
|
||||
// throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id);
|
||||
// }
|
||||
// auto device = dev_factory_->create<AbstractAGV>(node);
|
||||
// if (device == nullptr) {
|
||||
// throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
// }
|
||||
// devices_[id] = device;
|
||||
// std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init();
|
||||
// LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success";
|
||||
// }
|
||||
|
||||
config::CameraConfig camera_config;
|
||||
ConfigHelper::getCamerasConfig(camera_config);
|
||||
@ -296,20 +288,36 @@ void DeviceManager::init_devices_() {
|
||||
std::get<std::shared_ptr<AbstractDexHand>>(devices_[id])->init();
|
||||
}
|
||||
|
||||
auto robot_node = dmgr_node.getChild("Robot");
|
||||
for (auto &node: robot_node.getChildren()){
|
||||
string id = node.getAttrString("id");
|
||||
config::RobotConfig robot_config;
|
||||
ConfigHelper::getRobotConfig(robot_config);
|
||||
for (int i = 0; i < robot_config.humanrobots().size(); i++) {
|
||||
auto robot = robot_config.humanrobots(i);
|
||||
if (!robot.enable())
|
||||
continue;
|
||||
auto id = robot.id();
|
||||
if (devices_.count(id)) {
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate robot Device ID" << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate robot Device ID" + id);
|
||||
}
|
||||
auto device = dev_factory_->create<AbstractRobot>(node);
|
||||
if (device == nullptr) {
|
||||
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id);
|
||||
}
|
||||
std::cout << "DexHand[" << i << "]: " << id << std::endl;
|
||||
auto device = dev_factory_->createFromConfig<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;
|
||||
std::get<std::shared_ptr<AbstractRobot>>(devices_[id])->init();
|
||||
LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success";
|
||||
}
|
||||
|
||||
config::MicroPhoneConfig micro_phone_config;
|
||||
@ -346,22 +354,21 @@ void DeviceManager::init_devices_() {
|
||||
std::get<std::shared_ptr<AbstractSpeaker>>(devices_[id])->init();
|
||||
}
|
||||
|
||||
|
||||
auto biohead_node = dmgr_node.getChild("BioHead");
|
||||
for (auto &node: biohead_node.getChildren()){
|
||||
string id = node.getAttrString("id");
|
||||
if (devices_.count(id)){
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id);
|
||||
}
|
||||
auto device = dev_factory_->create<AbstractBiohead>(node);
|
||||
if (device == nullptr){
|
||||
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
config::HeadConfig head_cfg;
|
||||
ConfigHelper::getHeadConfig(head_cfg);
|
||||
for (int i = 0; i < head_cfg.bioheads().size(); i++) {
|
||||
auto head = head_cfg.bioheads(i);
|
||||
if (!head.enable())
|
||||
continue;
|
||||
auto id = head.id();
|
||||
if (devices_.count(id)) {
|
||||
LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id;
|
||||
throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id);
|
||||
}
|
||||
std::cout << "Biohead[" << i << "]: " << id << std::endl;
|
||||
auto device = dev_factory_->createFromConfig<AbstractBiohead,config::BioHeadConfig>(head);
|
||||
devices_[id] = device;
|
||||
std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init();
|
||||
LOG(INFO) << "[BioHead]: Init biohead " << id << " Success";
|
||||
|
||||
}
|
||||
}
|
||||
catch (const exception& e) {
|
||||
|
||||
@ -58,6 +58,7 @@ namespace cmvr::device {
|
||||
// 抽象头部类
|
||||
class AbstractBiohead : public AbstractDevice {
|
||||
public:
|
||||
AbstractBiohead() = default;
|
||||
explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {}
|
||||
~AbstractBiohead() override = default;
|
||||
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include "cmvr/config/biohead_config/biohead_config.pb.h"
|
||||
namespace cmvr::device {
|
||||
|
||||
struct ServoChannel {
|
||||
@ -18,6 +18,7 @@ namespace cmvr::device {
|
||||
class BioHeadRobot : public AbstractBiohead {
|
||||
public:
|
||||
explicit BioHeadRobot(const XmlNode &config);
|
||||
BioHeadRobot(const cmvr::config::BioHeadConfig& config);
|
||||
~BioHeadRobot() override = default;
|
||||
|
||||
// 接口实现
|
||||
@ -66,7 +67,7 @@ namespace cmvr::device {
|
||||
std::shared_ptr<std::thread> speak_thread_;
|
||||
std::atomic<bool> speak_running_{false};
|
||||
|
||||
|
||||
cmvr::config::BioHeadConfig config_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -26,6 +26,219 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) {
|
||||
|
||||
}
|
||||
|
||||
BioHeadRobot::BioHeadRobot(const cmvr::config::BioHeadConfig& config) : config_(config) {
|
||||
// 从config中读取串口参数
|
||||
port_name_ = config_.serial();
|
||||
|
||||
serial_ = std::make_shared<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) {
|
||||
|
||||
@ -21,7 +21,7 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
|
||||
return;
|
||||
}
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto &dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
|
||||
cam->start();
|
||||
|
||||
@ -54,6 +54,7 @@ namespace cmvr::device {
|
||||
|
||||
class AbstractCanbus : public AbstractDevice {
|
||||
public:
|
||||
AbstractCanbus() = default;
|
||||
AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {}
|
||||
~AbstractCanbus() {}
|
||||
/**
|
||||
|
||||
@ -35,6 +35,17 @@ namespace cmvr {
|
||||
// LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what();
|
||||
}
|
||||
|
||||
SocketCanClientRaw::SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config):config_(config)
|
||||
{
|
||||
auto channel_id = config_.channelid();
|
||||
port_ = static_cast<CANCardParameter::CANChannelId>(channel_id);
|
||||
interface_ = CANCardParameter::NATIVE;
|
||||
|
||||
enable_can_err_check_ = false;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SocketCanClientRaw::init() {
|
||||
// if (!parameter.has_channel_id()) {
|
||||
|
||||
@ -26,6 +26,7 @@
|
||||
#include "gflags/gflags.h"
|
||||
#include "../../abstract_canbus.h"
|
||||
#include "canbus/common/canbus_consts.h"
|
||||
#include "cmvr/config/robot_config/robot_config.pb.h"
|
||||
|
||||
namespace cmvr {
|
||||
namespace device {
|
||||
@ -36,6 +37,7 @@ namespace cmvr {
|
||||
class SocketCanClientRaw final : public AbstractCanbus {
|
||||
public:
|
||||
explicit SocketCanClientRaw(const XmlNode &cfg);
|
||||
SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config);
|
||||
~SocketCanClientRaw();
|
||||
void init() override;
|
||||
void start() override;
|
||||
@ -73,6 +75,8 @@ namespace cmvr {
|
||||
|
||||
//
|
||||
bool enable_can_err_check_{false};
|
||||
|
||||
cmvr::config::CanDeviceConfig config_;
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
add_subdirectory(ti5_motor)
|
||||
|
||||
add_subdirectory(eyou_motor)
|
||||
# --------------------------------------------------------
|
||||
# Unit test
|
||||
# --------------------------------------------------------
|
||||
|
||||
@ -41,6 +41,7 @@ namespace cmvr::device{
|
||||
} JointStatus;
|
||||
|
||||
public:
|
||||
AbstractMotor() = default;
|
||||
explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {}
|
||||
AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {}
|
||||
~AbstractMotor() override = default;
|
||||
|
||||
19
cmvr-es/devices/motor/eyou_motor/CMakeLists.txt
Normal file
19
cmvr-es/devices/motor/eyou_motor/CMakeLists.txt
Normal file
@ -0,0 +1,19 @@
|
||||
add_library(eyoumotor SHARED
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/ethercat/eyou_motor_ethercat_protocol.cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/eyou_motor.cpp
|
||||
|
||||
)
|
||||
|
||||
|
||||
target_include_directories(eyoumotor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
add_library(cmvr_es::device::eyoumotor ALIAS eyoumotor)
|
||||
|
||||
target_link_libraries(eyoumotor
|
||||
PRIVATE
|
||||
eu_ethercat
|
||||
protobuf
|
||||
glog
|
||||
)
|
||||
|
||||
install(TARGETS eyoumotor LIBRARY DESTINATION lib)
|
||||
@ -0,0 +1,652 @@
|
||||
//
|
||||
// Created by cmvr on 2026/3/30.
|
||||
//
|
||||
|
||||
#include "eyou_motor_ethercat_protocol.h"
|
||||
#include "canbus/canopen/register.h"
|
||||
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
|
||||
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
|
||||
#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
|
||||
#include "canbus/canopen/nmt_response_protocol.h"
|
||||
|
||||
#include <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);
|
||||
// 可以根据需要更新其他状态信息
|
||||
}
|
||||
@ -0,0 +1,106 @@
|
||||
//
|
||||
// Created by cmvr on 2026/3/30.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
#include "cmvr/msgs/motor.pb.h"
|
||||
#include "motor/motor_protocol_interface.h"
|
||||
#include "../../../canbus/abstract_canbus.h"
|
||||
#include "canbus/can_comm/can_receiver.h"
|
||||
#include "canbus/can_comm/can_sender.h"
|
||||
#include "canbus/can_comm/message_manager.h"
|
||||
#include "cmvr/msgs/error_code.pb.h"
|
||||
#include "cmvr/msgs/robot_detail.pb.h"
|
||||
|
||||
#include "eyou_ethercat/include/eu_ethercat.h"
|
||||
|
||||
#include <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
Normal file
5
cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp
Normal file
@ -0,0 +1,5 @@
|
||||
//
|
||||
// Created by cmvr on 2026/3/30.
|
||||
//
|
||||
|
||||
#include "eyou_motor.h"
|
||||
51
cmvr-es/devices/motor/eyou_motor/eyou_motor.h
Normal file
51
cmvr-es/devices/motor/eyou_motor/eyou_motor.h
Normal file
@ -0,0 +1,51 @@
|
||||
//
|
||||
// Created by cmvr on 2026/3/30.
|
||||
//
|
||||
#pragma once
|
||||
#include "../abstract_motor.h"
|
||||
#include "motor/motor_protocol_interface.h"
|
||||
#include "motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h"
|
||||
|
||||
namespace cmvr {
|
||||
namespace device {
|
||||
class EYouMotor : public AbstractMotor {
|
||||
public:
|
||||
EYouMotor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){}
|
||||
EYouMotor(const XmlNode &config):AbstractMotor(config) {
|
||||
info_.id = config.getAttrDefault("slave", -1);
|
||||
info_.joint_name = config.getAttrString("jointName");
|
||||
info_.limitQ = config.getAttrDefault("limitQ", 3.14f);
|
||||
info_.limitQd = config.getAttrDefault("limitQd", 0.5f);
|
||||
node_id_ = info_.id;
|
||||
}
|
||||
|
||||
EYouMotor(const cmvr::config::EthercatMotorConfig& config) : AbstractMotor() {
|
||||
info_.id = config.slave();
|
||||
info_.joint_name = config.jointname();
|
||||
info_.limitQ = config.limitqlb();
|
||||
info_.limitQd = config.limitqd();
|
||||
node_id_ = info_.id;
|
||||
}
|
||||
/**
|
||||
* 电机初始化在具体的类中完成
|
||||
*/
|
||||
void init() override {
|
||||
if (!protocol_) {
|
||||
throw std::runtime_error("Protocol not set for motor");
|
||||
}
|
||||
if (protocol_->comm_proto == MotorProtocolInterface::CommProto::ETHERCAT ) {
|
||||
auto canopen_protocol = std::dynamic_pointer_cast<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_);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@ -15,7 +15,8 @@ namespace cmvr {
|
||||
public:
|
||||
enum class CommProto : uint8_t {
|
||||
CANOPEN = 1,
|
||||
CUSTOM = 2
|
||||
ETHERCAT = 2,
|
||||
CUSTOM = 3
|
||||
};
|
||||
virtual ~MotorProtocolInterface() = default;
|
||||
|
||||
|
||||
@ -1,11 +1,12 @@
|
||||
//
|
||||
// Created by lgv on 2025/8/1.
|
||||
//
|
||||
#pragma once
|
||||
|
||||
#include "../abstract_motor.h"
|
||||
#include "motor/motor_protocol_interface.h"
|
||||
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||||
|
||||
#include "cmvr/config/robot_config/robot_config.pb.h"
|
||||
/**
|
||||
* class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成
|
||||
*/
|
||||
@ -22,6 +23,15 @@ namespace cmvr {
|
||||
node_id_ = info_.id;
|
||||
}
|
||||
|
||||
Ti5Motor(const cmvr::config::MotorConfig& config) : AbstractMotor() {
|
||||
info_.id = config.id();
|
||||
info_.joint_name = config.jointname();
|
||||
info_.limitQ = config.limitqlb();
|
||||
info_.limitQd = config.limitqd();
|
||||
node_id_ = info_.id;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 电机初始化在具体的类中完成
|
||||
*/
|
||||
|
||||
@ -41,6 +41,7 @@ namespace cmvr::device{
|
||||
|
||||
class AbstractRobot: public AbstractDevice {
|
||||
public:
|
||||
AbstractRobot() = default;
|
||||
explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
|
||||
~AbstractRobot() override=default;
|
||||
|
||||
|
||||
@ -35,6 +35,8 @@
|
||||
#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
|
||||
#include "planner/joint_space_planner/include/joint_space_planner.h"
|
||||
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
||||
#include "cmvr/config/robot_config/robot_config.pb.h"
|
||||
|
||||
|
||||
namespace cmvr::device{
|
||||
|
||||
@ -49,12 +51,10 @@ namespace cmvr::device{
|
||||
ROBOT_ERROR
|
||||
} RobotStateMachine;
|
||||
|
||||
// typedef enum {
|
||||
// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE
|
||||
// } RobotRunningMode;
|
||||
|
||||
public:
|
||||
explicit HumanoidRobot(const XmlNode& cfg);
|
||||
HumanoidRobot(const cmvr::config::HumanRobotConfig& config);
|
||||
HumanoidRobot(const cmvr::config::EthercatRobotConfig& config);
|
||||
~HumanoidRobot();
|
||||
|
||||
void init() override;
|
||||
@ -232,6 +232,7 @@ namespace cmvr::device{
|
||||
std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
|
||||
|
||||
|
||||
std::string urdf_path_ = "";
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -16,7 +16,7 @@ template<int DOF>
|
||||
HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
|
||||
try {
|
||||
id_ = cfg.getAttrString("id");
|
||||
|
||||
urdf_path_ = cfg.getAttrString("urdf");
|
||||
dof_ = DOF;
|
||||
if (!pathExists(cfg.getAttrString("urdf"))) {
|
||||
throw runtime_error("urdf file does not exist");
|
||||
@ -49,6 +49,87 @@ HumanoidRobot<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>
|
||||
void HumanoidRobot<DOF>::init() {
|
||||
motor_manager_ = MotorsInfo::getInstance()->getMotorManager();
|
||||
@ -59,7 +140,7 @@ void HumanoidRobot<DOF>::init() {
|
||||
std::vector<double>(7, 1.0));
|
||||
|
||||
|
||||
ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
|
||||
ik_solver_ = std::make_shared<PinocchioDlsIKSolver>(urdf_path_,
|
||||
"PELVIS_S",
|
||||
"R_WRIST_R_S",
|
||||
"R_FINGER_TIP_FIXED");
|
||||
|
||||
@ -112,7 +112,7 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
|
||||
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
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");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
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");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto robot_abs = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
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");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
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");
|
||||
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
|
||||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||||
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
|
||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam");
|
||||
@ -990,3 +990,21 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
|
||||
|
||||
EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps.";
|
||||
}
|
||||
|
||||
|
||||
TEST(HumanoidRobotTest, EYouTest) {
|
||||
auto& dmgr = DeviceManager::getInstance();
|
||||
auto robot_abs = dmgr.getDevice<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));
|
||||
}
|
||||
|
||||
@ -9,27 +9,13 @@
|
||||
#include <glog/logging.h>
|
||||
#include "service/grpc/include/server_runner.h"
|
||||
#include "utils/base/include/logger.h"
|
||||
#include "common/utils/config_helper/include/config_helper.h"
|
||||
using namespace cmvr::service;
|
||||
int main(int argc, char* argv[]) {
|
||||
std::string config_path;
|
||||
if(argc == 1) {
|
||||
char exe_path[PATH_MAX] = {0};
|
||||
ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
|
||||
if (count == -1) {
|
||||
std::cerr << "Failed to read /proc/self/exe" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
std::string exe_dir = dirname(exe_path);
|
||||
config_path = exe_dir + "/config/cabin_robot.xml";
|
||||
std::cout << "Using default config path: " << config_path << std::endl;
|
||||
}
|
||||
else if (argc == 2){
|
||||
config_path = argv[1];
|
||||
}
|
||||
|
||||
ServerRunner runner;
|
||||
const XmlNode config(config_path);
|
||||
runner.start(config);
|
||||
cmvr::config::ServerConfig server_config;
|
||||
cmvr::ConfigHelper::getServerConfig(server_config);
|
||||
ServerRunner runner(server_config);
|
||||
runner.start();
|
||||
runner.join();
|
||||
google::ShutdownGoogleLogging();
|
||||
return 0;
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
namespace cmvr::monitor{
|
||||
class AbstractMonitor {
|
||||
public:
|
||||
AbstractMonitor() = default;
|
||||
explicit AbstractMonitor(const XmlNode &cfg){
|
||||
cfg_ = cfg;
|
||||
freq_ = cfg_.getAttrDefault("freq", 1);
|
||||
|
||||
@ -8,17 +8,21 @@
|
||||
#include <condition_variable>
|
||||
|
||||
#include "../../abstract_monitor.h"
|
||||
#include "cmvr/config/monitor_config/monitor_config.pb.h"
|
||||
|
||||
namespace cmvr::monitor {
|
||||
|
||||
class DiskMonitor final : public AbstractMonitor{
|
||||
public:
|
||||
explicit DiskMonitor(const XmlNode& config);
|
||||
explicit DiskMonitor(const cmvr::config::DiskMonitorConfig& config); // 新增proto构造函数
|
||||
~DiskMonitor();
|
||||
protected:
|
||||
bool check_() override;
|
||||
bool execution_() override;
|
||||
private:
|
||||
void initFromProto(const cmvr::config::DiskMonitorConfig& config); // 新增初始化方法
|
||||
|
||||
std::unordered_map<std::string,int> folders_;
|
||||
std::vector<std::string> dirsToClean_; // 需要清理的目录列表
|
||||
mutable std::mutex mutex_; // 保护共享资源的互斥锁
|
||||
@ -27,6 +31,4 @@ namespace cmvr::monitor {
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //FOLDER_MONITOR_H
|
||||
@ -14,7 +14,6 @@ using namespace cmvr::monitor;
|
||||
namespace fs = std::filesystem;
|
||||
using namespace std::chrono;
|
||||
|
||||
|
||||
// 文件信息结构体
|
||||
struct FileInfo {
|
||||
std::string path;
|
||||
@ -38,51 +37,40 @@ bool deleteOldestFile(const std::string& directoryPath) {
|
||||
struct dirent* entry;
|
||||
struct stat fileStat;
|
||||
|
||||
// 遍历目录中的每个条目
|
||||
while ((entry = readdir(dir)) != nullptr) {
|
||||
// 跳过 "." 和 ".."
|
||||
if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 构建完整路径
|
||||
std::string filePath = directoryPath + "/" + entry->d_name;
|
||||
|
||||
// 获取文件状态信息
|
||||
if (stat(filePath.c_str(), &fileStat) == -1) {
|
||||
std::cerr << "Failed to get file status: " << filePath << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 只处理普通文件
|
||||
if (S_ISREG(fileStat.st_mode)) {
|
||||
FileInfo fileInfo;
|
||||
fileInfo.path = filePath;
|
||||
|
||||
// 优先使用 st_birthtime (如果可用)，否则使用 st_ctime
|
||||
#ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME
|
||||
fileInfo.creationTime = fileStat.st_birthtime;
|
||||
#else
|
||||
fileInfo.creationTime = fileStat.st_ctime;
|
||||
#endif
|
||||
|
||||
files.push_back(fileInfo);
|
||||
}
|
||||
}
|
||||
|
||||
closedir(dir);
|
||||
|
||||
// 如果没有找到文件，返回失败
|
||||
if (files.empty()) {
|
||||
std::cout << "No files found in directory: " << directoryPath << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 按创建时间排序
|
||||
std::sort(files.begin(), files.end(), compareByCreationTime);
|
||||
|
||||
// 删除最早创建的文件
|
||||
std::string oldestFilePath = files[0].path;
|
||||
|
||||
if (unlink(oldestFilePath.c_str()) == -1) {
|
||||
std::cerr << "Failed to delete file: " << oldestFilePath << std::endl;
|
||||
return false;
|
||||
@ -92,11 +80,11 @@ bool deleteOldestFile(const std::string& directoryPath) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool pathExists(const std::string& path) {
|
||||
struct stat buffer;
|
||||
return (stat(path.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
int countFilesInDirectory(const std::string& path) {
|
||||
DIR* dir = opendir(path.c_str());
|
||||
if (!dir) {
|
||||
@ -107,9 +95,7 @@ int countFilesInDirectory(const std::string& path) {
|
||||
int count = 0;
|
||||
struct dirent* entry;
|
||||
|
||||
// 遍历目录中的每个条目
|
||||
while ((entry = readdir(dir)) != nullptr) {
|
||||
// 跳过 "." 和 ".."
|
||||
if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") {
|
||||
count++;
|
||||
}
|
||||
@ -119,8 +105,8 @@ int countFilesInDirectory(const std::string& path) {
|
||||
return count;
|
||||
}
|
||||
|
||||
// XML构造函数
|
||||
DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){
|
||||
|
||||
try {
|
||||
for (auto &node: config.getChildren()){
|
||||
string fileDir = node.getAttrString("fileDir");
|
||||
@ -138,13 +124,42 @@ DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){
|
||||
throw runtime_error("[DiskMonitor] (DiskMonitor): Failed "+ string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
// Proto构造函数
|
||||
DiskMonitor::DiskMonitor(const cmvr::config::DiskMonitorConfig& config) : AbstractMonitor() {
|
||||
try {
|
||||
initFromProto(config);
|
||||
is_initialized_ = true;
|
||||
LOG(INFO) << "[DiskMonitor]: Init DiskMonitor from proto Success";
|
||||
}
|
||||
catch (const exception& e) {
|
||||
LOG(ERROR) << "[DiskMonitor] (DiskMonitor): Failed " << e.what();
|
||||
throw runtime_error("[DiskMonitor] (DiskMonitor): Failed " + string(e.what()));
|
||||
}
|
||||
}
|
||||
|
||||
// Proto初始化方法
|
||||
void DiskMonitor::initFromProto(const cmvr::config::DiskMonitorConfig& config) {
|
||||
// 设置监控ID和频率（通过AbstractMonitor）
|
||||
// 假设AbstractMonitor有setId和setFreq方法，如果没有需要添加
|
||||
// 这里根据你的AbstractMonitor实现来调整
|
||||
|
||||
for (const auto& folder : config.folders()) {
|
||||
string fileDir = folder.filedir();
|
||||
if (!pathExists(fileDir)) {
|
||||
throw runtime_error("[DiskMonitor] (initFromProto): path not exists: " + fileDir);
|
||||
}
|
||||
int maxVolume = folder.maxvolume();
|
||||
folders_[fileDir] = maxVolume;
|
||||
}
|
||||
}
|
||||
|
||||
DiskMonitor::~DiskMonitor() {
|
||||
folders_.clear();
|
||||
}
|
||||
|
||||
bool DiskMonitor::check_() {
|
||||
// 检查当前目录下的文件数量是否超过最大值
|
||||
bool allOk = true;
|
||||
// 遍历所有监控的文件夹
|
||||
for (const auto& folder : folders_) {
|
||||
const std::string& path = folder.first;
|
||||
size_t maxFiles = folder.second;
|
||||
@ -154,28 +169,21 @@ bool DiskMonitor::check_() {
|
||||
continue;
|
||||
}
|
||||
int nFileCount = countFilesInDirectory(path);
|
||||
if (nFileCount > maxFiles)
|
||||
{
|
||||
if (nFileCount > maxFiles) {
|
||||
allOk = false;
|
||||
dirsToClean_.push_back(path);
|
||||
std::cout << "path need remove oldest file: " << path << std::endl;
|
||||
}
|
||||
|
||||
|
||||
} catch (const fs::filesystem_error& e) {
|
||||
std::cerr << "Filesystem error: " << e.what() << '\n';
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Unexpected error: " << e.what() << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
return allOk;
|
||||
}
|
||||
|
||||
|
||||
bool DiskMonitor::execution_() {
|
||||
|
||||
// 删除创建时间最早的文件
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool success = true;
|
||||
|
||||
@ -183,6 +191,6 @@ bool DiskMonitor::execution_() {
|
||||
deleteOldestFile(path);
|
||||
}
|
||||
|
||||
dirsToClean_.clear(); // 清理完成后清空列表
|
||||
dirsToClean_.clear();
|
||||
return success;
|
||||
}
|
||||
@ -5,8 +5,8 @@
|
||||
#ifndef MONITOR_FACTORY_H
|
||||
#define MONITOR_FACTORY_H
|
||||
|
||||
|
||||
#include "../../monitor/abstract_monitor.h"
|
||||
#include "monitor/abstract_monitor.h"
|
||||
#include "cmvr/config/monitor_config/monitor_config.pb.h"
|
||||
|
||||
namespace cmvr::monitor
|
||||
{
|
||||
@ -14,14 +14,17 @@ namespace cmvr::monitor
|
||||
{
|
||||
public:
|
||||
MonitorFactory() = default;
|
||||
|
||||
// 原有的XML方式
|
||||
template <typename MonitorType>
|
||||
std::shared_ptr<MonitorType> 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
|
||||
@ -15,23 +15,38 @@ namespace cmvr::monitor {
|
||||
public:
|
||||
MonitorManager(const MonitorManager&) = delete;
|
||||
MonitorManager& operator=(const MonitorManager&) = delete;
|
||||
|
||||
// 原有的XML方式
|
||||
static MonitorManager& getInstance(const XmlNode &cfg);
|
||||
|
||||
// 新增proto方式
|
||||
static MonitorManager& getInstance(const cmvr::config::MonitorManagerConfig& config);
|
||||
|
||||
static MonitorManager& getInstance();
|
||||
static void destroyInstance();
|
||||
|
||||
private:
|
||||
explicit MonitorManager (const XmlNode &cfg);
|
||||
void init_monitors();
|
||||
// 原有的XML方式
|
||||
explicit MonitorManager(const XmlNode &cfg);
|
||||
|
||||
// 新增proto方式
|
||||
explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config);
|
||||
|
||||
void init_monitors(); // XML方式
|
||||
void init_monitors_from_proto(); // Proto方式
|
||||
|
||||
private:
|
||||
static std::once_flag init_flag_;
|
||||
static std::shared_ptr<MonitorManager> instance_;
|
||||
|
||||
XmlNode cfg_;
|
||||
// 两种配置方式，只有一个有效
|
||||
bool use_proto_ = false;
|
||||
XmlNode cfg_; // XML配置
|
||||
cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置
|
||||
|
||||
std::shared_ptr<MonitorFactory> monitor_factory_;
|
||||
std::unordered_map<std::string, std::shared_ptr<AbstractMonitor>> monitors_;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif //MONITOR_MANAGER_H
|
||||
@ -3,12 +3,12 @@
|
||||
//
|
||||
|
||||
#include "../include/monitor_factory.h"
|
||||
|
||||
#include "../../monitor/diskmonitor/include/disk_monitor.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace cmvr::monitor;
|
||||
|
||||
// XML方式的模板特化
|
||||
template std::shared_ptr<DiskMonitor> MonitorFactory::create<DiskMonitor>(const XmlNode&);
|
||||
|
||||
template <typename MonitorType>
|
||||
@ -17,8 +17,30 @@ std::shared_ptr<MonitorType> MonitorFactory::create(const XmlNode& cfg) {
|
||||
return std::make_shared<DiskMonitor>(cfg);
|
||||
}
|
||||
else {
|
||||
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
|
||||
throw runtime_error("[DeviceFactory]: Unsupported device type");
|
||||
LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type";
|
||||
throw runtime_error("[MonitorFactory]: Unsupported monitor type");
|
||||
}
|
||||
}
|
||||
|
||||
// Proto方式的模板特化
|
||||
template std::shared_ptr<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);
|
||||
}
|
||||
@ -3,8 +3,7 @@
|
||||
//
|
||||
|
||||
#include "../include/monitor_manager.h"
|
||||
|
||||
#include "../../monitor/diskmonitor/include/disk_monitor.h"
|
||||
#include "monitor/diskmonitor/include/disk_monitor.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace cmvr::monitor;
|
||||
@ -12,11 +11,19 @@ using namespace cmvr::monitor;
|
||||
std::shared_ptr<MonitorManager> MonitorManager::instance_ = nullptr;
|
||||
std::once_flag MonitorManager::init_flag_;
|
||||
|
||||
MonitorManager::MonitorManager(const XmlNode& cfg) {
|
||||
// XML方式构造函数
|
||||
MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) {
|
||||
monitor_factory_ = make_shared<MonitorFactory>();
|
||||
cfg_ = cfg;
|
||||
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) {
|
||||
std::call_once(init_flag_, [&cfg] {
|
||||
instance_.reset(new MonitorManager(cfg));
|
||||
@ -24,9 +31,17 @@ MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) {
|
||||
return *instance_;
|
||||
}
|
||||
|
||||
// Proto方式单例
|
||||
MonitorManager& MonitorManager::getInstance(const cmvr::config::MonitorManagerConfig& config) {
|
||||
std::call_once(init_flag_, [&config] {
|
||||
instance_.reset(new MonitorManager(config));
|
||||
});
|
||||
return *instance_;
|
||||
}
|
||||
|
||||
MonitorManager& MonitorManager::getInstance() {
|
||||
if (!instance_) {
|
||||
throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first.");
|
||||
throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance() first.");
|
||||
}
|
||||
return *instance_;
|
||||
}
|
||||
@ -35,13 +50,14 @@ void MonitorManager::destroyInstance() {
|
||||
instance_.reset();
|
||||
}
|
||||
|
||||
// XML方式初始化
|
||||
void MonitorManager::init_monitors() {
|
||||
try{
|
||||
try {
|
||||
auto diskmonitor_node = cfg_.getChild("DiskMonitor");
|
||||
auto id = diskmonitor_node.getAttrString("id");
|
||||
auto monitor = monitor_factory_->create<DiskMonitor>(diskmonitor_node);
|
||||
if (monitor == nullptr) {
|
||||
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
|
||||
throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id);
|
||||
}
|
||||
monitors_[id] = monitor;
|
||||
}
|
||||
@ -49,5 +65,31 @@ void MonitorManager::init_monitors() {
|
||||
LOG(ERROR) << e.what();
|
||||
throw runtime_error(e.what());
|
||||
}
|
||||
}
|
||||
|
||||
// Proto方式初始化
|
||||
void MonitorManager::init_monitors_from_proto() {
|
||||
try {
|
||||
if (!proto_cfg_.has_disk_monitor()) {
|
||||
LOG(WARNING) << "[MonitorManager]: No disk_monitor config found";
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& disk_monitor_cfg = proto_cfg_.disk_monitor();
|
||||
if (!disk_monitor_cfg.enable()) {
|
||||
LOG(INFO) << "[MonitorManager]: Disk monitor is disabled";
|
||||
return;
|
||||
}
|
||||
|
||||
auto id = disk_monitor_cfg.id();
|
||||
auto monitor = monitor_factory_->create<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());
|
||||
}
|
||||
}
|
||||
@ -11,13 +11,14 @@
|
||||
#include <thread>
|
||||
#include <grpcpp/grpcpp.h>
|
||||
#include "rapidxml/xml_parser.h"
|
||||
#include "cmvr/config/server_config/server_config.pb.h"
|
||||
namespace cmvr::service {
|
||||
|
||||
|
||||
class ServerRunner {
|
||||
public:
|
||||
ServerRunner();
|
||||
explicit ServerRunner(XmlNode cfg);
|
||||
ServerRunner(const cmvr::config::ServerConfig& config);
|
||||
~ServerRunner();
|
||||
|
||||
ServerRunner(const ServerRunner&) = delete;
|
||||
@ -25,7 +26,7 @@ namespace cmvr::service {
|
||||
ServerRunner(ServerRunner&&) = delete;
|
||||
ServerRunner& operator=(ServerRunner&&) = delete;
|
||||
|
||||
void start(XmlNode cfg);
|
||||
void start();
|
||||
void stop();
|
||||
void join();
|
||||
|
||||
@ -40,7 +41,7 @@ namespace cmvr::service {
|
||||
std::condition_variable cv_;
|
||||
std::thread worker_;
|
||||
|
||||
XmlNode cfg_;
|
||||
cmvr::config::ServerConfig cfg_;
|
||||
std::unique_ptr<grpc::Server> server_;
|
||||
|
||||
std::string address_;
|
||||
|
||||
@ -17,7 +17,7 @@ using google::protobuf::util::TimeUtil;
|
||||
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance())
|
||||
{
|
||||
|
||||
pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
|
||||
pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
|
||||
,"PELVIS_S"
|
||||
,"R_WRIST_R_S");
|
||||
pinocchio_ik_->init();
|
||||
|
||||
@ -25,23 +25,21 @@
|
||||
using namespace cmvr::service;
|
||||
ServerRunner::ServerRunner() = default;
|
||||
|
||||
ServerRunner::ServerRunner(XmlNode cfg) {
|
||||
start(std::move(cfg));
|
||||
ServerRunner::ServerRunner(const cmvr::config::ServerConfig& config):cfg_(config)
|
||||
{
|
||||
// start();
|
||||
}
|
||||
|
||||
ServerRunner::~ServerRunner() {
|
||||
stop();
|
||||
join();
|
||||
}
|
||||
|
||||
void ServerRunner::start(XmlNode cfg) {
|
||||
void ServerRunner::start() {
|
||||
std::unique_lock<std::mutex> lk(mtx_);
|
||||
|
||||
if (worker_.joinable()) {
|
||||
throw std::runtime_error("ServerRunner already started");
|
||||
}
|
||||
|
||||
cfg_ = std::move(cfg);
|
||||
stop_requested_ = false;
|
||||
started_ = false;
|
||||
start_failed_ = false;
|
||||
@ -97,25 +95,15 @@ void ServerRunner::threadMain() {
|
||||
using namespace cmvr::monitor;
|
||||
|
||||
try {
|
||||
if (!cfg_.hasChild("DeviceManager")) {
|
||||
throw std::runtime_error("Device Manager node not found");
|
||||
}
|
||||
|
||||
|
||||
static std::once_flag reflection_once;
|
||||
std::call_once(reflection_once, []() {
|
||||
grpc::reflection::InitProtoReflectionServerBuilderPlugin();
|
||||
});
|
||||
DeviceManager::getInstance();
|
||||
|
||||
auto dmgr_cfg = cfg_.getChild("DeviceManager");
|
||||
DeviceManager::getInstance(dmgr_cfg);
|
||||
|
||||
if (cfg_.hasChild("MonitorManager")) {
|
||||
auto mmgr_cfg = cfg_.getChild("MonitorManager");
|
||||
MonitorManager::getInstance(mmgr_cfg);
|
||||
}
|
||||
|
||||
auto grpc_cfg = cfg_.getChild("gRPCServer");
|
||||
std::string port = grpc_cfg.getAttrDefault("port", "50051");
|
||||
std::string port = cfg_.grpc_server().port();
|
||||
std::string local_address = "0.0.0.0:" + port;
|
||||
|
||||
auto camera_service = std::make_unique<gRPCCameraServiceImpl>();
|
||||
|
||||
44
protos/cmvr/config/biohead_config/biohead_config.proto
Normal file
44
protos/cmvr/config/biohead_config/biohead_config.proto
Normal file
@ -0,0 +1,44 @@
|
||||
// biohead.proto
|
||||
syntax = "proto3";
|
||||
|
||||
package cmvr.config;
|
||||
|
||||
// 眉毛配置
|
||||
message EyeBrowConfig {
|
||||
string serial = 1; // 如 "64:0~3"
|
||||
string offest = 2; // 偏移量，如 "90 90 90 90"
|
||||
string jLmtUp = 3; // 上限，如 "90 170 155 110"
|
||||
string jLmtLow = 4; // 下限，如 "20 77 90 20"
|
||||
}
|
||||
|
||||
// 眼睛配置
|
||||
message EyeConfig {
|
||||
string serial = 1; // 如 "64:4~9"
|
||||
string offest = 2; // 偏移量，如 "90 90 90 90 90 90"
|
||||
string jLmtUp = 3; // 上限，如 "90 150 165 90 120 115"
|
||||
string jLmtLow = 4; // 下限，如 "20 90 90 25 70 75"
|
||||
}
|
||||
|
||||
// 嘴巴配置
|
||||
message MouthConfig {
|
||||
string serial = 1; // 如 "65:0~9"
|
||||
string offest = 2; // 偏移量，如 "90 90 90 90 90 90 90 90 90 90"
|
||||
string jLmtUp = 3; // 上限，如 "150 110 130 140 100 105 110 125 90 95"
|
||||
string jLmtLow = 4; // 下限，如 "70 30 80 80 65 55 45 80 85 90"
|
||||
}
|
||||
|
||||
// BioHead配置
|
||||
message BioHeadConfig {
|
||||
string id = 1; // 设备ID，如 "bio_head"
|
||||
string serial = 2; // 串口路径，如 "/dev/ttyUSB0"
|
||||
int32 ctrlFreq = 3; // 控制频率，如 50
|
||||
|
||||
EyeBrowConfig eyebrow = 4; // 眉毛配置
|
||||
EyeConfig eye = 5; // 眼睛配置
|
||||
MouthConfig mouth = 6; // 嘴巴配置
|
||||
bool enable = 7; // 是否启用
|
||||
}
|
||||
|
||||
message HeadConfig {
|
||||
repeated BioHeadConfig bioheads = 3;
|
||||
}
|
||||
22
protos/cmvr/config/monitor_config/monitor_config.proto
Normal file
22
protos/cmvr/config/monitor_config/monitor_config.proto
Normal file
@ -0,0 +1,22 @@
|
||||
syntax = "proto3";
|
||||
|
||||
package cmvr.config;
|
||||
|
||||
// 文件夹监控配置
|
||||
message FolderConfig {
|
||||
string fileDir = 1; // 文件夹路径
|
||||
int32 maxVolume = 2; // 最大容量（MB）
|
||||
}
|
||||
|
||||
// 磁盘监控配置
|
||||
message DiskMonitorConfig {
|
||||
string id = 1; // 监控器ID，如 "file_monitor"
|
||||
int32 freq = 2; // 监控频率（Hz）
|
||||
repeated FolderConfig folders = 3; // 要监控的文件夹列表
|
||||
bool enable = 4; // 是否启用
|
||||
}
|
||||
|
||||
// 监控管理器配置
|
||||
message MonitorManagerConfig {
|
||||
DiskMonitorConfig disk_monitor = 1; // 磁盘监控配置
|
||||
}
|
||||
88
protos/cmvr/config/robot_config/robot_config.proto
Normal file
88
protos/cmvr/config/robot_config/robot_config.proto
Normal file
@ -0,0 +1,88 @@
|
||||
syntax = "proto3";
|
||||
|
||||
package cmvr.config;
|
||||
|
||||
// 电机配置
|
||||
message MotorConfig {
|
||||
int32 id = 1; // 电机ID
|
||||
string jointName = 2; // 关节名称
|
||||
float limitQLb = 3; // 位置下限
|
||||
float limitQUb = 4; // 位置上限
|
||||
float limitQd = 5; // 速度限制
|
||||
}
|
||||
|
||||
// 通用的CAN设备配置（手臂、头部、腰部都可以复用）
|
||||
message CanDeviceConfig {
|
||||
string id = 1; // CAN ID
|
||||
string devId = 2; // 设备ID
|
||||
int32 channelId = 3; // 通道ID
|
||||
bool enable = 4; // 是否启用
|
||||
string toolFrame = 5; // 工具坐标系（手臂需要，头/腰可选）
|
||||
repeated MotorConfig motors = 6; // 电机列表
|
||||
}
|
||||
|
||||
// CAN管理器配置
|
||||
message CanManagerConfig {
|
||||
string id = 1; // CAN管理器ID
|
||||
string devId = 2; // 设备ID
|
||||
CanDeviceConfig left_arm = 3; // 左臂CAN
|
||||
CanDeviceConfig right_arm = 4; // 右臂CAN
|
||||
CanDeviceConfig head = 5; // 头部CAN
|
||||
CanDeviceConfig waist = 6; // 腰部CAN
|
||||
}
|
||||
|
||||
// 人形机器人配置
|
||||
message HumanRobotConfig {
|
||||
string id = 1; // 机器人ID，如 "hc01"
|
||||
int32 dof = 2; // 自由度，如 14
|
||||
string urdf = 3; // URDF文件路径
|
||||
string baseLink = 4; // 基座link名称，如 "PELVIS_S"
|
||||
string jointNames = 5; // 关节名称列表，逗号分隔
|
||||
string linkNames = 6; // link名称列表，逗号分隔
|
||||
int32 bufferSize = 7; // 缓冲区大小，如 50
|
||||
bool verbose = 8; // 是否详细输出
|
||||
int32 updFreq = 9; // 更新频率，如 500
|
||||
CanManagerConfig can_manager = 10; // CAN管理器配置
|
||||
bool enable = 11; // 是否启用
|
||||
}
|
||||
|
||||
// EtherCAT主站配置
|
||||
message EthercatMasterConfig {
|
||||
string interface = 1; // 网络接口，如 "eth0"
|
||||
int32 cycle_time_us = 2; // 周期时间（微秒），默认1000
|
||||
bool enable = 3; // 是否启用
|
||||
}
|
||||
|
||||
// EtherCAT电机驱动配置
|
||||
message EthercatMotorConfig {
|
||||
int32 slave = 1; // 从站ID
|
||||
string jointName = 2; // 关节名称
|
||||
float limitQLb = 3; // 位置下限
|
||||
float limitQUb = 4; // 位置上限
|
||||
float limitQd = 5; // 速度限制
|
||||
}
|
||||
|
||||
// EtherCAT机器人配置
|
||||
message EthercatRobotConfig {
|
||||
string id = 1; // 机器人ID
|
||||
int32 dof = 2; // 自由度
|
||||
string urdf = 3; // URDF文件路径
|
||||
string baseLink = 4; // 基座link名称
|
||||
string jointNames = 5; // 关节名称列表，逗号分隔
|
||||
string linkNames = 6; // link名称列表，逗号分隔
|
||||
int32 bufferSize = 7; // 缓冲区大小
|
||||
bool verbose = 8; // 是否详细输出
|
||||
int32 updFreq = 9; // 更新频率（Hz）
|
||||
bool enable = 10; // 是否启用
|
||||
|
||||
EthercatMasterConfig master = 11; // EtherCAT主站配置
|
||||
repeated EthercatMotorConfig motors = 12; // 电机配置列表
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 主配置
|
||||
message RobotConfig {
|
||||
repeated HumanRobotConfig humanrobots = 1;
|
||||
repeated EthercatRobotConfig ethercat_robots = 2; // 新增的EtherCAT机器人
|
||||
}
|
||||
10
protos/cmvr/config/server_config/server_config.proto
Normal file
10
protos/cmvr/config/server_config/server_config.proto
Normal file
@ -0,0 +1,10 @@
|
||||
syntax = "proto3";
|
||||
package cmvr.config;
|
||||
|
||||
message GrpcServerConfig{
|
||||
string port = 1;
|
||||
}
|
||||
|
||||
message ServerConfig{
|
||||
GrpcServerConfig grpc_server = 1;
|
||||
}
|
||||
@ -33,5 +33,6 @@ third_party/modbus/3.1.11
|
||||
third_party/visp/3.7.0
|
||||
third_party/mainif/0.0.5
|
||||
third_party/matplotplusplus/1.2.0
|
||||
third_party/eyou_ethercat
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user