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

revert: rollback dev to fb304df94c

Browse Source
This commit is contained in:
lgv 2026-06-25 09:26:23 +08:00
parent b4468809da
commit a6d749f679
89 changed files with 123 additions and 12838 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -122,8 +122,6 @@ target_link_libraries(cmvr_es PRIVATE
cmvr_es::device::humanoid_robot
cmvr_es::common
cmvr_es::applications
cmvr_es::device::agv_src1100
)
install(TARGETS cmvr_es RUNTIME DESTINATION bin)

30
cmvr-es/common/config/agv_config/agv_config.pb.txt
View File

@ -1,30 +0,0 @@
src1100_agvs {
id: "agv_src1100"
ip: "192.168.192.5"
port_status: 19204
port_control: 19205
port_nav: 19206
port_config: 19207
port_other: 19210
port_push: 19302
enable: true
}
src1100_agvs {
id: "agv_src2200"
ip: "192.168.192.5"
port_status: 19204
port_control: 19205
port_nav: 19206
port_config: 19207
port_other: 19210
port_push: 19301
enable: true
}

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

@ -1,27 +0,0 @@
bioheads {
id: "bio_head"
serial: "/dev/ttyUSB0"
ctrlFreq: 50
enable: false
eyebrow {
serial: "64:0~3"
offest: "90 90 90 90"
jLmtUp: "90 170 155 110"
jLmtLow: "20 77 90 20"
}
eye {
serial: "64:4~9"
offest: "90 90 90 90 90 90"
jLmtUp: "90 150 165 90 120 115"
jLmtLow: "20 90 90 25 70 75"
}
mouth {
serial: "65:0~9"
offest: "90 90 90 90 90 90 90 90 90 90"
jLmtUp: "150 110 130 140 100 105 110 125 90 95"
jLmtLow: "70 30 80 80 65 55 45 80 85 90"
}
}

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

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

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

@ -1,6 +1,6 @@
rh56dftp_dexhands {
id: "hand1"
ip: "192.168.1.223"
ip: "192.168.1.213"
port: 6000
poll_interval_ms: 10
enable: false
@ -11,7 +11,7 @@ rh56dftp_dexhands {
ip: "192.168.1.224"
port: 6000
poll_interval_ms: 10
enable: false
enable: true
}
px_6ax_gen3 {
@ -32,5 +32,5 @@ px_6ax_gen3 {
sensor_name: "Paxini Gen3末端压力"
polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE
auto_calibrate: false
enable: false
enable: true
}

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

@ -1,25 +0,0 @@
# 监控管理器配置
monitor_manager {
disk_monitor {
id: "file_monitor"
freq: 1
enable: false
folders {
fileDir: "/home/share/assets/audio"
maxVolume: 1000
}
folders {
fileDir: "/home/share/assets/image"
maxVolume: 1000
}
folders {
fileDir: "/home/share/assets/video"
maxVolume: 1000
}
folders {
fileDir: "../log"
maxVolume: 1000
}
}
}

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

@ -1,36 +0,0 @@
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
}
motors {
slave: 2
jointName: "L_SHOULDER_R"
limitQLb: -3.14
limitQUb: 3.14
limitQd: 3.0
}
}

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

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

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

@ -10,10 +10,6 @@
#include "cmvr/config/microphone_config/microphone_config.pb.h"
#include "cmvr/config/speaker_config/speaker_conifg.pb.h"
#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
#include "cmvr/config/agv_config/agv_config.grpc.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 { \
@ -86,26 +82,6 @@ namespace cmvr
return SET_CONFIG(config, touch_screen_app_config_file);
}
static bool getAgvConfig(config::AGVConfig& config)
{
return GET_CONFIG(agv_config_file, config);
}
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>

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

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

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

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

53
cmvr-es/common/utils/ffmpeg/include/FrameData.h
View File

@ -1,53 +0,0 @@
// FrameData.h
#pragma once
#include <opencv2/opencv.hpp>
extern "C" {
#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
}
namespace ffmpeg {
struct FrameData {
AVFrame* av_frame = nullptr; // 编码用帧（YUV格式）
cv::Mat rgb_image; // OpenCV用图像（BGR格式）
int64_t pts = 0; // 时间戳
int64_t frame_index = 0; // 帧序号
FrameData() = default;
~FrameData() {
if (av_frame) {
av_frame_free(&av_frame);
}
}
// 禁止拷贝
FrameData(const FrameData&) = delete;
FrameData& operator=(const FrameData&) = delete;
// 允许移动
FrameData(FrameData&& other) noexcept
: av_frame(other.av_frame),
rgb_image(std::move(other.rgb_image)),
pts(other.pts),
frame_index(other.frame_index) {
other.av_frame = nullptr;
}
FrameData& operator=(FrameData&& other) noexcept {
if (this != &other) {
if (av_frame) av_frame_free(&av_frame);
av_frame = other.av_frame;
rgb_image = std::move(other.rgb_image);
pts = other.pts;
frame_index = other.frame_index;
other.av_frame = nullptr;
}
return *this;
}
};
} // namespace ffmpeg

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

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

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

@ -23,9 +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;

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

@ -3,8 +3,6 @@
//
#include "data_center/include/motors_info.h"
#include "motor/eyou_motor/eyou_motor.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h"
@ -96,10 +94,7 @@ void MotorsInfo::init(const XmlNode &cfg) {
};
// 创建 MotorManager
if (!motor_manager_)
{
motor_manager_ = std::make_shared<MotorManager>();
}
std::vector<std::future<void> > tasks;
@ -156,155 +151,6 @@ void MotorsInfo::init(const XmlNode &cfg) {
LOG(INFO) << "All enabled motors initialized successfully.";
}
void MotorsInfo::init(const cmvr::config::HumanRobotConfig& config) {
const auto& can_manager = config.can_manager();
// 读取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_) {
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>>();
}
// 右臂
if (right_arm_enabled_) {
const auto& right_arm = can_manager.right_arm();
for (const auto& motor : right_arm.motors()) {
r_motors_cfg.push_back(motor);
}
r_can_client_ = std::make_shared<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>>();
}
// 腰部
if (waist_enabled_) {
const auto& waist = can_manager.waist();
for (const auto& motor : waist.motors()) {
waist_motors_cfg.push_back(motor);
}
waist_can_client_ = std::make_shared<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>>();
}
// 头部
if (head_enabled_) {
const auto& head = can_manager.head();
for (const auto& motor : head.motors()) {
head_motors_cfg.push_back(motor);
}
head_can_client_ = std::make_shared<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<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}
};
if (!motor_manager_)
{
motor_manager_ = std::make_shared<MotorManager>();
}
std::vector<std::future<void>> tasks;
for (auto &limb : limbs) {
if (!limb.enabled) continue;
if (limb.client) limb.client->init();
if (limb.sender && limb.receiver && limb.client) {
auto ret = limb.sender->Init(limb.client.get(), false);
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to init " << limb.name << " CAN sender.";
ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false);
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver.";
limb.client->start();
ret = limb.sender->Start();
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to start " << limb.name << " CAN sender.";
ret = limb.receiver->Start();
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
}
std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
if (limb.sender && limb.message_manager) {
protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
}
if (!limb.motor_cfgs.empty()) {
tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name << " motors...";
for (const auto& motor_cfg : limb.motor_cfgs) {
auto motor = std::make_shared<Ti5Motor>(motor_cfg);
if (protocol) motor->setProtocol(protocol);
motor->init();
motor_manager_->addMotor(motor);
}
}));
}
}
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};

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

@ -15,7 +15,6 @@ target_link_libraries(device_manager PRIVATE
cmvr_es::device::rh56dftp_dexhand
cmvr_es::device::px_6ax_gen3
cmvr::device::head_esp32
cmvr_es::device::agv_src1100
osqp
cmvr_es::device::humanoid_robot
# cmvr_es::device::aubo_robot

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

@ -28,7 +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<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);

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

@ -16,7 +16,7 @@
namespace cmvr::device {
using DeviceVariant = std::variant<
std::shared_ptr<AbstractAgv>,
std::shared_ptr<AbstractAGV>,
std::shared_ptr<AbstractBattery>,
std::shared_ptr<AbstractCamera>,
std::shared_ptr<AbstractDexHand>,

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

@ -15,13 +15,12 @@
//#include "robot/ti5_robot/ti5_robot.h"
#include "cmvr/api/system_command.pb.h"
#include "data_center/include/motors_info.h"
#include "devices/agv/agv_src1100/include/agv_src1100.h"
//#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<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&);
@ -33,7 +32,7 @@ template std::shared_ptr<AbstractBiohead> DeviceFactory::create<AbstractBiohead>
template <typename DeviceType>
std::shared_ptr<DeviceType> DeviceFactory::create(const XmlNode& cfg) {
if constexpr (std::is_same_v<DeviceType, AbstractAgv>) {
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);
@ -58,7 +57,7 @@ std::shared_ptr<DeviceType> DeviceFactory::create(const XmlNode& cfg) {
}
}
std::shared_ptr<AbstractAgv> DeviceFactory::create_agv_(const XmlNode& cfg) {
std::shared_ptr<AbstractAGV> DeviceFactory::create_agv_(const XmlNode& cfg) {
return nullptr;
}
@ -185,11 +184,6 @@ template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<Abstra
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<AbstractAgv> DeviceFactory::createFromConfig<AbstractAgv>(const config::AGVsrc1100Config& 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)
{
@ -250,50 +244,6 @@ 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 if constexpr (std::is_same_v<DeviceType, AbstractAgv>)
{
if constexpr (std::is_same_v<ConfigType, config::AGVsrc1100Config>)
{
return std::make_shared<AgvSrc1100>(cfg);
}
else
{
LOG(ERROR) << "[DeviceFactory]: Unsupported agv device type ";
throw runtime_error("[DeviceFactory]: Unsupported agv device type");
}
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[DeviceFactory]: Unsupported device type");

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

@ -9,7 +9,7 @@ using namespace std;
using namespace cmvr::device;
using namespace cmvr::device;
template std::shared_ptr<AbstractAgv> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractBattery> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractCamera> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractDexHand> DeviceManager::getDevice(const std::string& device_id);
@ -152,7 +152,7 @@ void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>
string dev_type;
std::visit([&](const auto& ptr) {
using T = std::decay_t<decltype(ptr)>;
if constexpr (std::is_same_v<T, std::shared_ptr<AbstractAgv>>) {
if constexpr (std::is_same_v<T, std::shared_ptr<AbstractAGV>>) {
dev_type = "AGV";
} else if constexpr (std::is_same_v<T, std::shared_ptr<AbstractBattery>>) {
dev_type = "Battery";
@ -223,12 +223,12 @@ void DeviceManager::init_devices_() {
LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id);
}
auto device = dev_factory_->create<AbstractAgv>(node);
auto device = dev_factory_->create<AbstractAGV>(node);
if (device == nullptr) {
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
}
devices_[id] = device;
std::get<std::shared_ptr<AbstractAgv>>(devices_[id])->init();
std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init();
LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success";
}
@ -312,24 +312,6 @@ void DeviceManager::init_devices_() {
LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success";
}
// EYOU的先用新版的
config::RobotConfig robot_config;
ConfigHelper::getRobotConfig(robot_config);
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();
}
config::MicroPhoneConfig micro_phone_config;
ConfigHelper::getMicroPhonesConfig(micro_phone_config);
for (int i = 0; i < micro_phone_config.ffmpeg_microphones().size(); i++) {
@ -364,38 +346,22 @@ void DeviceManager::init_devices_() {
std::get<std::shared_ptr<AbstractSpeaker>>(devices_[id])->init();
}
config::HeadConfig head_cfg;
ConfigHelper::getHeadConfig(head_cfg);
for (int i = 0; i < head_cfg.bioheads().size(); i++) {
auto head = head_cfg.bioheads(i);
if (!head.enable())
continue;
auto id = head.id();
if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id);
auto biohead_node = dmgr_node.getChild("BioHead");
for (auto &node: biohead_node.getChildren()){
string id = node.getAttrString("id");
if (devices_.count(id)){
LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id);
}
auto device = dev_factory_->create<AbstractBiohead>(node);
if (device == nullptr){
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
}
std::cout << "Biohead[" << i << "]: " << id << std::endl;
auto device = dev_factory_->createFromConfig<AbstractBiohead,config::BioHeadConfig>(head);
devices_[id] = device;
std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init();
}
config::AGVConfig agv_config;
ConfigHelper::getAgvConfig(agv_config);
LOG(INFO) << "[BioHead]: Init biohead " << id << " Success";
for (int i = 0; i < agv_config.src1100_agvs().size(); i++) {
auto agv = agv_config.src1100_agvs(i);
if (!agv.enable())
continue;
auto id = agv.id();
if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate agv Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate agv Device ID" + id);
}
std::cout << "AgvSrc1100[" << i << "]: " << id << std::endl;
auto device = dev_factory_->createFromConfig<AbstractAgv,config::AGVsrc1100Config>(agv);
devices_[id] = device;
std::get<std::shared_ptr<AbstractAgv>>(devices_[id])->init();
}
}
catch (const exception& e) {

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

@ -6,4 +6,3 @@ add_subdirectory(biohead)
add_subdirectory(robot)
add_subdirectory(canbus)
add_subdirectory(motor)
add_subdirectory(agv)

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

@ -1,2 +0,0 @@
add_subdirectory(agv_src1100)

881
cmvr-es/devices/agv/abstract_agv.h
View File

@ -1,869 +1,36 @@
//
// Created by xtkuang on 2025/5/6.
// Created by xtkuang on 2025/5/8.
//
/**
* @file abstract_agv.h
* @brief AGV设备抽象基类定义，提供统一的接口规范和数据结构。
* 所有具体AGV实现（如SRC-1100）需继承此类。
* @note 本文件定义了大量与仙工API对应的结构体，用于状态查询、运动控制、导航任务等。
*/
#ifndef CMVR_ES_ABSTRACT_AGV_H
#define CMVR_ES_ABSTRACT_AGV_H
#pragma once
#include <cmath>
#include <cstdint>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include <json/json.h>
#include "../abstract_device.h"
#include "devices/abstract_device.h"
namespace cmvr::device{
class AbstractAGV: public AbstractDevice {
public:
explicit AbstractAGV(const XmlNode &config): AbstractDevice(config) {};
~AbstractAGV() override=default;
// 注意：这里只include，不使用
#include "cmvr/config/agv_config/agv_config.pb.h"
virtual void getState(AGVState &state) {}
namespace cmvr::device {
// navigation
virtual void eStop() {}
virtual void goHome() {}
virtual void moveto(math::Pose2d &location, double speed_ratio) {}
virtual void setVelocity(math::Vec3 linear, math::Vec3 angular) {}
/**
* @brief AGV（自动导引车）设备的抽象基类
*
* 继承自 AbstractDevice，定义了 AGV 设备通用的接口和行为，
* 包括状态管理、信息查询、运动控制、地图管理、导航任务等功能。
* 所有具体 AGV 设备实现必须继承此类并实现其纯虚函数。
*/
class AbstractAgv : public AbstractDevice {
public:
/**
* @brief AGV 设备运行状态枚举
*/
enum class Status {
CREATED, ///< 已创建，未初始化
INITIALIZED, ///< 已初始化，未运行
RUNNING, ///< 运行中
PAUSED, ///< 暂停
STOPPED, ///< 已停止
FAULT ///< 故障
// map
virtual void initMap(float resolution, int width, int height) {}
virtual void updateMap() {}
virtual void saveMap(const std::string& file_path) {}
virtual void loadMap(const std::string& file_path) {}
protected:
AGVState state_;
};
}
// ==================== 状态与信息结构体 ====================
/**
* @brief 机器人基本信息查询结果
*
* 对应仙工 API 1000 (0x03E8) 的响应字段
*/
struct AgvStatusInfo {
std::string id; ///< 设备唯一标识
std::string vehicle_id; ///< 车辆编号（如 "agv_001"）
std::string robot_note; ///< 机器人备注（用户自定义）
std::string version; ///< 软件版本号
std::string model; ///< 车型（如 "SRC-1100"）
std::string dsp_version; ///< DSP 固件版本
std::string gyro_version; ///< 陀螺仪版本
std::string map_version; ///< 地图版本
std::string model_version; ///< 模型版本
std::string netprotocol_version; ///< 网络协议版本
std::string modbus_version; ///< Modbus 协议版本
std::string current_map; ///< 当前加载的地图名称
std::string current_map_md5; ///< 当前地图的 MD5 校验值
std::string model_md5; ///< 模型文件 MD5
std::string ssid; ///< 当前连接的 Wi-Fi SSID
int rssi = 0; ///< Wi-Fi 信号强度 (RSSI, 0~100)
std::string ap_addr; ///< 接入点 MAC 地址
std::string current_ip; ///< 当前 IP 地址
std::string mac; ///< 设备 MAC 地址
std::string echoid_type; ///< 回音 ID 类型
std::string echoid; ///< 回音 ID
int ret_code = 0; ///< 返回码，0 表示成功，非0 表示错误
std::string err_msg; ///< 错误信息（失败时填充）
/**
* @brief 检查信息是否有效（ID 非空且返回码为 0）
*/
bool isValid() const {
return !id.empty() && ret_code == 0;
}
};
/**
* @brief 电池状态信息
*
* 对应仙工 API 1007 (0x03EF) 的响应字段
*/
struct BatteryStatus
{
double battery_level = 0.0; ///< 电池电量百分比 (0~100)
double battery_temp = 0.0; ///< 电池温度（摄氏度）
bool charging = false; ///< 是否正在充电
double voltage = 0.0; ///< 当前电压（V）
double current = 0.0; ///< 当前电流（A）
double max_charge_voltage = -1.0; ///< 最大充电电压（-1 表示未知）
double max_charge_current = -1.0; ///< 最大充电电流（-1 表示未知）
bool manual_charge = false; ///< 是否连接手动充电器（仅 SRC-2000）
bool auto_charge = false; ///< 是否连接自动充电桩（仅 SRC-2000）
int32_t battery_cycle = 0; ///< 电池循环次数（从 BMS 读取，可能不准确）
std::string battery_user_data; ///< 电池用户自定义数据（字符串）
std::string extra; ///< 额外扩展字段（预留）
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳（ISO 8601）
std::string err_msg; ///< 错误信息
};
/**
* @brief 机器人位置信息
*
* 对应仙工 API 1004 (0x03EC) 的响应字段
*/
struct RobotLocation
{
double x = 0.0; ///< 世界坐标系 X 坐标（米）
double y = 0.0; ///< 世界坐标系 Y 坐标（米）
double angle = 0.0; ///< 朝向角（弧度），逆时针为正
double confidence = 0.0; ///< 定位置信度（0~1）
std::string current_station; ///< 当前所在站点 ID（若在站点上）
std::string last_station; ///< 上一个经过的站点 ID
int32_t loc_method = 0; ///< 定位方法（0: 自然轮廓, 1: 反光柱, 2: 二维码, 3: 里程计, ...）
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 地图下载结果
*
* 对应仙工 API 4011 (0x0FAB) 的响应
*/
struct DownloadMapResult
{
std::string map_name; ///< 地图名称
std::string map_content; ///< 地图文件内容（JSON 字符串）
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 单个地图文件信息
*/
struct MapFileInfo
{
std::string name; ///< 文件名
std::string modified; ///< 最后修改时间
int64_t size = 0; ///< 文件大小（字节）
};
/**
* @brief 地图整体状态（当前地图、所有地图列表及文件详情）
*
* 对应仙工 API 1300 (0x0514) 的响应
*/
struct MapStatus
{
std::string current_map; ///< 当前加载的地图名称
std::string current_map_md5; ///< 当前地图的 MD5
std::vector<std::string> maps; ///< 所有地图名称列表
std::vector<MapFileInfo> map_files_info; ///< 每个地图文件的详细信息
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 上传地图结果
*
* 对应仙工 API 4010 (0x0FAA) 的响应
*/
struct UploadMapResult
{
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 抢占控制权返回结果
*
* 对应仙工 API 4005 (0x0FA5) 的响应
*/
struct LockResult
{
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 当前控制权信息
*
* 对应仙工 API 1060 (0x0424) 的响应
*/
struct CurrentLockStatus
{
bool locked = false; ///< 是否被锁定（有控制权持有者）
std::string ip; ///< 持有者 IP 地址
int32_t port = 0; ///< 持有者端口
uint8_t type = 0; ///< 持有者类型（0: 未知, 1: 调度系统, 2: 示教器, ...）
std::string nick_name; ///< 持有者昵称（自定义标识）
int64_t time_t = 0; ///< 锁定时间戳（Unix 秒）
std::string desc; ///< 描述信息
int32_t ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
// ==================== 运动控制相关结构体 ====================
/**
* @brief 开环运动下发参数
*
* 对应仙工 API 2010 (0x07DA) 的请求参数
* 注意：多舵轮设备仅 vx/vy/w 生效，steer/real_steer 仅单舵轮设备有效。
*/
struct MotionCtrlReq
{
double vx = 0.0; ///< 机器人坐标系 X 方向线速度（m/s）
double vy = 0.0; ///< 机器人坐标系 Y 方向线速度（m/s）
double w = 0.0; ///< 角速度（rad/s），逆时针为正
double steer = 0.0; ///< 舵轮转向角（仅单舵轮，单位 rad）
double real_steer = 0.0; ///< 实际舵轮角度（反馈，仅单舵轮）
int64_t duration = -1; ///< 运动持续时间（毫秒），-1 表示一直运动直到下次指令
};
/**
* @brief 运动控制返回结果
*/
struct MotionCtrlRes
{
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 切换地图入参
*
* 对应仙工 API 2022 (0x07E6) 的请求
*/
struct LoadMapReq
{
std::string map_name; ///< 目标地图名称（必须已存在于机器人中）
};
/**
* @brief 切换地图返回结果
*/
struct LoadMapRes
{
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 查询地图载入状态返回
*
* 对应仙工 API 1022 (0x03FE) 的响应
*/
struct QueryLoadMapStatusRes
{
int32_t loadmap_status = 0; ///< 地图加载状态：0=失败, 1=成功, 2=载入中
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 单个站点信息
*
* 对应仙工 API 1301 (0x0515) 响应中的单个站点对象
*/
struct StationItem
{
std::string id; ///< 站点 ID（唯一标识）
std::string type; ///< 站点类型（如 "LocationMark", "ChargePoint", "ActionPoint"）
double x = 0.0; ///< 站点 X 坐标（米）
double y = 0.0; ///< 站点 Y 坐标（米）
double r = 0.0; ///< 站点朝向角（弧度）
std::string desc; ///< 描述信息
std::string executor; ///< 执行器信息（用于动作）
std::string prepoint; ///< 前置点 ID（若需要先到前置点）
std::string recfile; ///< 识别文件路径（用于视觉识别）
bool spin = false; ///< 是否允许自旋（原地旋转）
bool use_down_pgv = false; ///< 是否使用下视 PGV 二维码
};
/**
* @brief 查询站点列表返回结果
*/
struct QueryStationRes
{
std::vector<StationItem> stations; ///< 站点列表
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
/**
* @brief 单段导航任务（用于指定路径导航）
*
* 对应仙工 API 3066 (0x0BFA) 中 move_task_list 的每个元素
*/
struct MoveTaskItem
{
std::string task_id; ///< 任务 ID（必须全局唯一）
std::string source_id; ///< 起始站点 ID
std::string id; ///< 目标站点 ID
std::string operation; ///< 到达后执行的动作（如 "JackLoad", "WaitDI", 等）
double jack_height = 0.0; ///< 顶升高度（当 operation 为顶升相关时使用）
};
/**
* @brief 指定路径导航入参（多个导航段）
*
* 对应仙工 API 3066 的请求数据区
*/
struct GoTargetListReq
{
std::vector<MoveTaskItem> move_task_list; ///< 导航任务序列（必须按顺序且相邻站点间有直接路径）
};
/**
* @brief 指定路径导航返回结果
*/
struct GoTargetListRes
{
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
// ==================== 导航料箱信息 ====================
/**
* @brief 导航任务中涉及的料箱信息
*/
struct NavContainerItem
{
std::string container_name; ///< 料箱名称
std::string desc; ///< 描述
std::string goods_id; ///< 绑定的货物 ID
bool has_goods = false; ///< 是否已绑定货物
};
// ==================== 1020 实时导航查询 ====================
/**
* @brief 1020 实时导航查询入参
*
* 对应仙工 API 1020 (0x03FC)
*/
struct RobotStatusTaskCurrentReq
{
bool simple = false; ///< 若为 true，只返回 task_status 字段
};
/**
* @brief 1020 实时导航查询出参
*
* 对应仙工 API 1020 的响应
*/
struct RobotStatusTaskCurrentRes
{
int task_status = 0; ///< 任务状态：0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED
int task_type = 0; ///< 任务类型：0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点, 7=平动转动, 100=其他
std::string target_id; ///< 目标站点 ID（若 task_type 为 2 或 3）
std::vector<double> target_point; ///< 目标坐标 [x, y, r]（若 task_type 为 1）
std::vector<std::string> finished_path; ///< 已经过的站点列表
std::vector<std::string> unfinished_path; ///< 尚未经过的站点列表
std::string move_status_info; ///< 导航附加信息（如错误描述）
std::vector<NavContainerItem> containers; ///< 料箱信息（若有）
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
// ==================== 1110 批量任务查询 ====================
/**
* @brief 单条任务状态项
*/
struct SingleTaskStatusItem
{
std::string task_id; ///< 任务 ID
int status = 0; ///< 状态码（同上述 task_status）
int type = 0; ///< 任务类型（同上述 task_type）
};
/**
* @brief 任务状态包（包含最近完成和所有未完成的任务）
*
* 对应仙工 API 1110 (0x0456) 的响应中的 task_status_package
*/
struct TaskStatusPackage
{
std::string closest_target; ///< 最近站点 ID
std::string source_name; ///< 起点站点名称
std::string target_name; ///< 终点站点名称
double percentage = 0.0; ///< 任务进度百分比（0~100）
double distance = 0.0; ///< 剩余距离（米）
std::string info; ///< 附加信息
std::vector<SingleTaskStatusItem> task_status_list; ///< 所有任务状态列表
};
/**
* @brief 1110 批量查询任务入参
*/
struct QueryTaskStatusPackageReq
{
std::vector<std::string> task_ids; ///< 要查询的任务 ID 列表，若为空则查询所有未完成+最近一条完成
};
/**
* @brief 1110 批量查询任务出参
*/
struct QueryTaskStatusPackageRes
{
TaskStatusPackage task_status_package; ///< 任务状态包
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
// ==================== DI / DO / 音频 / PGV / 脚本等通用类型 ====================
/**
* @brief DI 点位信息
*/
struct DIItem
{
int id = 0; ///< DI 编号
bool status = false; ///< 状态（true=高电平, false=低电平）
};
/**
* @brief DO 点位信息
*/
struct DOItem
{
int id = 0; ///< DO 编号
bool status = false; ///< 状态（true=高电平, false=低电平）
};
/**
* @brief 音频播放参数
*/
struct SoundArgs
{
std::string name; ///< 音频文件名
int loop = 0; ///< 循环次数（0=播放一次, 1=循环播放）
int stop = 0; ///< 1=停止播放
};
/**
* @brief WaitDI 动作参数
*/
struct WaitDIArgs
{
std::vector<DIItem> DI; ///< 需要等待的 DI 条件列表
double timeout = 0.0; ///< 超时时间（秒），0 表示无限等待
};
/**
* @brief SetDO 动作参数
*/
struct SetDOArgs
{
std::vector<DOItem> DO; ///< 要设置的 DO 列表
};
/**
* @brief PGV（二维码）二次定位参数
*/
struct PgvParam
{
bool use_pgv = false; ///< 是否使用上视 PGV
bool use_down_pgv = false; ///< 是否使用下视 PGV
double pgv_adjust_dist = 0.0; ///< 最大调整半径（米）
double pgv_adjust_cx = 0.0; ///< 二维码坐标系下的 X 偏移（米）
double pgv_adjust_cy = 0.0; ///< 二维码坐标系下的 Y 偏移（米）
double pgv_x_adjust = 0.0; ///< 仅调整 X 方向误差（米）
};
/**
* @brief 自由导航目标点（x, y, theta）
*/
struct FreeGoPoint
{
double x = 0.0; ///< X 坐标（米）
double y = 0.0; ///< Y 坐标（米）
double theta = 0.0; ///< 朝向角（弧度）
};
/**
* @brief 脚本参数（通用）
*/
struct ScriptArgs
{
std::map<std::string, std::string> str_kv; ///< 字符串键值对
std::map<std::string, double> num_kv; ///< 数字键值对
std::vector<DOItem> do_list; ///< DO 列表（用于 setDO 等）
std::vector<DIItem> di_list; ///< DI 列表（用于 waitDI 等）
};
// ==================== 3051 单点导航 ====================
/**
* @brief 3051 单点导航入参
*
* 对应仙工 API 3051 (0x0BEB) 的请求数据区
*
* @attention
* - **仅用于单车测试/验证，严禁用于多车调度场景**，否则会出现速度不连续、不跟随路径等危险。
* - 下发此任务会取消当前正在执行的任务（不排队）。
* - 成功下发后会自动清除指定的 warning/error 报错码。
* - 支持自由导航（freeGo）和基于站点的路径导航两种模式。
*/
struct RobotGoTargetReq
{
// -------- 必填字段 --------
std::string source_id; ///< 起始站点 ID（若为 "SELF_POSITION" 表示从当前位置出发）
std::string id; ///< 目标站点 ID（若为 "SELF_POSITION" 表示原地执行 operation）
std::string task_id; ///< 任务 ID（非强制，但建议提供）
// -------- 可选速度/角度控制 --------
double angle = 0.0; ///< 目标点朝向角（弧度），仅对自由导航或目标站点有方向要求时使用
std::string method; ///< 运动方式："forward"（正走）或 "backward"（倒走）
double max_speed = 0.0; ///< 最大线速度（m/s），0 表示使用默认
double max_wspeed = 0.0; ///< 最大角速度（rad/s）
double max_acc = 0.0; ///< 最大加速度（m/s²）
double max_wacc = 0.0; ///< 最大角加速度（rad/s²）
int64_t duration = 0; ///< 到达后等待时间（毫秒）
int32_t orientation = 0; ///< 全向车保持的角度（暂不使用）
bool spin = false; ///< 是否随动（用于托盘旋转）
int64_t delay = 0; ///< 延迟结束导航状态的时间（毫秒），缺省为 0
int32_t start_rot_dir = 0; ///< 起步原地旋转方向：-1=顺时针, 0=就近, 1=逆时针
int32_t end_rot_dir = 0; ///< 到点原地旋转方向（同上）
double reach_dist = 0.0; ///< 到点位置精度（米），默认由系统决定
double reach_angle = 0.0; ///< 到点角度精度（弧度）
std::string skill_name; ///< 技能名称："Action"（动作）或 "GotoSpecifiedPose"（路径导航）
// -------- PGV 二次定位 --------
PgvParam pgv; ///< PGV 参数
// -------- 设备操作（顶升/货叉/辊筒/牵引等） --------
std::string operation; ///< 操作类型，如 "JackLoad", "ForkUnload", "RollerLoad", "HookLoad", "WaitDI", "SetDO", "sound", "Script" 等
double jack_height = 0.0; ///< 顶升高度（米），用于 JackLoad/JackUnload/JackHeight
double start_height = 0.0; ///< 货叉起步前举升高度（米）
double end_height = 0.0; ///< 货叉到点后举升高度（米）
double fork_mid_height = 0.0; ///< 货叉行走过程中举升高度（米）
double fork_dist = 0.0; ///< 货叉前移距离（米），用于 ForkForward
std::string direction; ///< 方向："left"/"right"/"front"/"back"，用于辊筒或货叉
bool recognize = false; ///< 是否启用视觉识别（用于货叉/顶升加载）
std::string recfile; ///< 识别文件路径（如 "shelf/s0002.shelf"）
SoundArgs sounds_args; ///< 音频参数（当 operation="sound" 时）
WaitDIArgs wait_di_args; ///< WaitDI 参数
SetDOArgs set_do_args; ///< SetDO 参数
std::string script_name; ///< 脚本文件名（当 operation="Script" 时）
ScriptArgs script_args; ///< 脚本参数
int32_t script_stage = 2; ///< 脚本执行阶段：0=导航前, 1=导航中, 2=导航后, 3=脚本控制导航
// -------- 原地旋转（特殊用法） --------
double move_angle = 0.0; ///< 旋转角度（弧度），仅当 skill_name="GoByOdometer" 时使用
double speed_w = 0.0; ///< 旋转角速度（rad/s）
int32_t loc_mode = 0; ///< 定位模式：1=激光定位, 0=里程定位
// -------- 自由导航（仅支持双轮差速底盘） --------
FreeGoPoint freeGo; ///< 自由导航目标坐标（若提供此字段，则忽略 id 字段，使用坐标导航）
};
/**
* @brief 3051 单点导航出参
*/
struct RobotGoTargetRes
{
int ret_code = 0; ///< 返回码，0 表示成功
std::string create_on; ///< 数据创建时间戳
std::string err_msg; ///< 错误信息
};
//新增暂停导航响应结构体
struct RobotTaskPauseRes {
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 继续导航响应结构体
struct RobotTaskResumeRes {
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// ==================== robot_control_stop 停止开环运动响应 ====================
// 0x07D0 robot_control_stop 停止开环运动返回
struct RobotControlStopRes
{
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 取消导航响应结构体
struct RobotTaskCancelRes {
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// ==================== 类型别名（兼容旧代码） ====================
using AgvState = AgvStatusInfo;
public:
AbstractAgv() = default;
~AbstractAgv() override = default;
// 禁用拷贝，允许移动
AbstractAgv(const AbstractAgv&) = delete;
AbstractAgv& operator=(const AbstractAgv&) = delete;
AbstractAgv(AbstractAgv&&) = default;
AbstractAgv& operator=(AbstractAgv&&) = default;
// ==================== 纯虚接口 ====================
/**
* @brief 获取当前设备运行状态
* @return Status 枚举值
*/
virtual Status state() const = 0;
/**
* @brief 获取最后一次发生的错误信息
* @return 错误描述字符串
*/
virtual std::string lastError() const = 0;
/**
* @brief 启动设备（建立连接、初始化资源等）
*/
void start() override = 0;
/**
* @brief 停止设备（断开连接、释放资源等）
*/
void stop() override = 0;
/**
* @brief 周期性更新设备状态（由上层循环调用，可处理心跳、重连等）
*/
void update() override = 0;
/**
* @brief 获取 AGV 状态（封装了 getStatusInfo）
* @param state 输出参数，填充 AgvState 结构
*/
virtual void getState(AgvState& state) {
state = AgvState{};
getStatusInfo(state);
}
/**
* @brief 获取 AGV 基本信息（命令码 1000）
* @param info 输出参数，填充 AgvStatusInfo
*/
virtual void getStatusInfo(AgvStatusInfo& info) = 0;
/**
* @brief 查询电池状态（命令码 1007）
* @param info 输出参数，填充 BatteryStatus
* @param simple 若为 true，只查询关键信息（可提高效率，具体取决于实现）
*/
virtual void getBatteryStatus(BatteryStatus& info, bool simple = false) = 0;
/**
* @brief 查询机器人当前位置（命令码 1004）
* @param info 输出参数，填充 RobotLocation
*/
virtual void getRobotLocation(RobotLocation& info) = 0;
/**
* @brief 下载指定名称的地图（命令码 4011）
* @param info 输出参数，填充 DownloadMapResult（包含地图内容）
* @param map_name 要下载的地图名称
*/
virtual void downloadMap(DownloadMapResult& info, const std::string& map_name) = 0;
/**
* @brief 查询地图状态（命令码 1300）
* @param info 输出参数，填充 MapStatus（当前地图、所有地图列表及文件信息）
*/
virtual void getMapStatus(MapStatus& info) = 0;
/**
* @brief 上传地图（命令码 4010）
* @param info 输出参数，填充 UploadMapResult
* @param map_json 地图 JSON 字符串（完整的地图文件内容）
*/
virtual void uploadMap(UploadMapResult& info, const std::string& map_json) = 0;
/**
* @brief 抢占机器人控制权（命令码 4005）
* @param info 输出参数，填充 LockResult
* @param nick_name 抢占者昵称（用于标识调用方）
*/
virtual void lockRobotControl(LockResult& info, const std::string& nick_name) = 0;
/**
* @brief 下发开环速度运动指令（命令码 2010）
*
* 使用控制端口（19205）。下发后会强制中断当前自动导航任务。
* 对于多舵轮设备，仅 vx/vy/w 生效。
*
* @param res 输出参数，运动指令返回结果
* @param req 输入参数，包含 vx/vy/w/steer/real_steer/duration
*/
virtual void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) = 0;
/**
* @brief 切换载入地图（命令码 2022）
*
* 使用控制端口（19205）。目标地图必须已在机器人中存储。
*
* @param res 输出参数，执行结果
* @param req 输入参数，目标地图名称
*/
virtual void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) = 0;
/**
* @brief 查询当前导航状态（命令码 1020）
*
* 使用状态端口（19204）。
* @param res 输出参数，导航全量状态
* @param req 输入参数，simple=true 仅返回 task_status
*/
virtual void robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& req) = 0;
/**
* @brief 简单运动控制（设置速度）
*
* 默认实现抛出 logic_error，子类可重写。
* @param vx X 方向速度 (m/s)
* @param vy Y 方向速度 (m/s)
* @param vw 角速度 (rad/s)
*/
virtual void move(double vx, double vy, double vw) {
throw std::logic_error("move not implemented");
}
/**
* @brief 停止运动
*
* 默认实现抛出 logic_error，子类可重写。
*/
virtual void stopMove() {
throw std::logic_error("stopMove not implemented");
}
/**
* @brief 查询当前机器人控制权持有者（命令码 1060）
* @param info 输出参数，填充 CurrentLockStatus
*/
virtual void getCurrentLockStatus(CurrentLockStatus& info) = 0;
/**
* @brief 查询地图载入状态（命令码 1022）
*
* 使用状态端口（19204）。
* @param res 输出参数，包含 loadmap_status（0=失败, 1=成功, 2=载入中）
* @note 载入中（2）时禁止执行重定位操作
*/
virtual void queryLoadMapStatus(QueryLoadMapStatusRes& res) = 0;
/**
* @brief 查询当前地图全部站点信息（命令码 1301）
*
* 使用状态端口（19204）。
* @param res 输出参数，包含站点数组与执行结果
*/
virtual void queryStationList(QueryStationRes& res) = 0;
/**
* @brief 指定路径导航（命令码 3066）
*
* 使用导航端口（19206）。任务串行追加执行。
*
* @param res 输出参数，下发任务结果（ret_code=0 仅表示接收成功，不代表执行完成）
* @param req 输入参数，多段站点导航序列（每个任务必须包含 task_id, source_id, id）
* @attention
* - source_id 和 id 之间必须有直接相连的线路，不可以跳点。
* - 任务会排队执行，若前一个任务失败，后续任务自动取消。
* - 适合多车调度场景，因为路径可预知、不会自主绕路。
*/
virtual void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) = 0;
/**
* @brief 批量查询任务状态（命令码 1110）
*
* 使用状态端口（19204）。
* @param res 输出参数，包含所有任务状态包（已完成+未完成）
* @param req 输入参数，要查询的 task_id 列表；若为空，则查询所有未完成+最近一条完成的任务
*/
virtual void robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) = 0;
/**
* @brief 单点站点自动规划导航（命令码 3051）
*
* 使用导航端口（19206）。
* @param res 输出参数，下发结果（ret_code=0 仅表示接收成功）
* @param req 完整导航参数（支持自由导航、固定路径导航、带设备操作等）
* @warning
* - **严禁用于多车调度场景**，仅限单车测试/任务链验证。
* - 下发新任务会取消当前正在执行的任务（不排队）。
* - 成功下发后会**自动清除**指定的 warning/error 报错码（详见协议文档）。
* - 支持自由导航（freeGo），但仅限双轮差速底盘。
*/
virtual void robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) = 0;
/**
* @brief 2000(0x07D0) robot_control_stop_req 停止开环速度运动指令
* @param res 出参，执行结果
*/
virtual void robotControlStop(RobotControlStopRes& res) = 0;
/**
* @brief 暂停当前导航任务（命令码 3001, 0x0BB9）
* @param res 输出参数，执行结果
*/
virtual void robotTaskPause(RobotTaskPauseRes& res) = 0;
/**
* @brief 继续当前导航任务（命令码 3002, 0x0BBA）
* @param res 输出参数，执行结果
*/
virtual void robotTaskResume(RobotTaskResumeRes& res) = 0;
/**
* @brief 取消当前导航任务（命令码 3003, 0x0BBB）
* @param res 输出参数，执行结果
*/
virtual void robotTaskCancel(RobotTaskCancelRes& res) = 0;
protected:
// ==================== 保护成员变量 ====================
std::string id_; ///< 设备 ID（唯一标识）
std::string ip_; ///< 设备 IP 地址
// 多端口配置（对应 proto 定义）
int port_status_{0}; ///< 状态查询端口（默认 19204）
int port_control_{0}; ///< 控制 API 端口（默认 19205）
int port_nav_{0}; ///< 导航 API 端口（默认 19206）
int port_config_{0}; ///< 配置 API 端口（默认 19207）
int port_other_{0}; ///< 其他 API 端口（默认 19210）
int port_push_{0}; ///< 推送端口（默认 19301）
bool enable_{false}; ///< 设备启用标志
};
} // namespace cmvr::device
#endif // CMVR_ES_ABSTRACT_AGV_H
#endif //CMVR_ES_ABSTRACT_AGV_H

20
cmvr-es/devices/agv/agv_src1100/CMakeLists.txt
View File

@ -1,20 +0,0 @@
add_library(agv_src1100 SHARED src/agv_src1100.cpp)
target_include_directories(agv_src1100 PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/include
${JSONCPP_INCLUDE_DIRS}
)
add_library(cmvr_es::device::agv_src1100 ALIAS agv_src1100)
# 关键修改：PRIVATE -> PUBLIC
target_link_libraries(agv_src1100 PUBLIC
jsoncpp
cmvr_es::common
cmvr_es::proto
glog
pthread
)
install(TARGETS agv_src1100 LIBRARY DESTINATION lib)

307
cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h
View File

@ -1,307 +0,0 @@
//
// Created by linbo on 2025/6/20.
//
/**
* @file agv_src1100.h
* @brief 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现类的头文件。
* 继承自 AbstractAgv，实现了所有纯虚接口，并管理多端口 TCP 连接。
*/
#ifndef AGV_SRC1100_H
#define AGV_SRC1100_H
#pragma once
#include "./agv/abstract_agv.h"
#include <mutex>
#include <atomic>
#include <thread>
#include "nlohmann/json.hpp"
using json = nlohmann::json;
namespace cmvr::device {
/**
* @brief 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现类
*
* 继承自 AbstractAgv，实现了与仙工智能 SRC 系列控制器通信的具体协议。
* 支持多端口（状态、控制、导航、配置、推送）并发连接和指令交互。
*
* @note 该类不是线程安全的，外部调用需自行保证同一设备实例的串行访问。
*/
class AgvSrc1100 final : public AbstractAgv {
public:
// ==================== 构造 / 析构 ====================
/**
* @brief 从 XML 配置节点构造设备实例（已弃用或未使用，保留兼容）
* @param cfg XML 配置节点，包含设备 ID、IP、端口、启用标志等
*/
explicit AgvSrc1100(const XmlNode& cfg);
/**
* @brief 从 Protobuf 配置对象构造设备实例
* @param cfg AGVsrc1100Config 配置对象，包含设备 ID、IP、端口、启用标志等
*/
explicit AgvSrc1100(const config::AGVsrc1100Config& cfg);
/**
* @brief 析构函数，自动停止设备并释放资源
*/
~AgvSrc1100() override;
// ==================== 生命周期管理（通用） ====================
/**
* @brief 获取当前设备运行状态
* @return Status 枚举值（CREATED / INITIALIZED / RUNNING / PAUSED / STOPPED / FAULT）
*/
Status state() const override;
/**
* @brief 获取最后一次发生的错误信息
* @return 错误描述字符串，若无错误则返回空字符串
*/
std::string lastError() const override;
/**
* @brief 启动设备，标记为运行状态
* @note 所有 socket 连接应在构造时已完成，启动仅改变状态标志
*/
void start() override;
/**
* @brief 停止设备，关闭所有 TCP 连接并释放资源
*/
void stop() override;
/**
* @brief 周期性更新设备状态
* @note 当前为空实现，可扩展心跳检测或重连逻辑
*/
void update() override;
// ==================== 端口 19204 – 机器人状态 API（允许 10 个连接） ====================
// 功能：查询机器人各种状态信息（只读操作，不改变机器人状态）
/**
* @brief 查询机器人基本信息（命令码 1000, robot_status_info_req）
* @param info 输出参数，填充 AgvStatusInfo
* @note 返回信息包括：版本、型号、地图名称、网络 IP、MAC、Wi-Fi 信号等
*/
void getStatusInfo(AgvStatusInfo& info) override;
/**
* @brief 查询电池状态（命令码 1007, robot_status_battery_req）
* @param info 输出参数，填充 BatteryStatus
* @param simple 若为 true，仅返回关键电量信息（当前实现未区分）
* @note 返回信息包括：电量百分比、温度、充放电状态、电压、电流等
*/
void getBatteryStatus(BatteryStatus& info, bool simple = false) override;
/**
* @brief 查询机器人当前位置（命令码 1004, robot_status_loc_req）
* @param info 输出参数，填充 RobotLocation
* @note 返回信息包括：世界坐标系 X/Y 坐标、朝向角、定位置信度、当前站点
*/
void getRobotLocation(RobotLocation& info) override;
/**
* @brief 查询地图状态（命令码 1300, robot_status_map_req）
* @param info 输出参数，填充 MapStatus
* @note 返回信息包括：当前载入的地图名称、所有存储的地图列表、文件详情
*/
void getMapStatus(MapStatus& info) override;
/**
* @brief 查询当前控制权持有者（命令码 1060, robot_status_current_lock_req）
* @param info 输出参数，填充 CurrentLockStatus
* @note 返回信息包括：是否被锁定、持有者 IP/端口/昵称、锁定时间等
*/
void getCurrentLockStatus(CurrentLockStatus& info) override;
/**
* @brief 查询地图载入状态（命令码 1022, robot_status_loadmap_req）
* @param res 输出参数，包含 loadmap_status
* @note loadmap_status: 0=失败, 1=成功, 2=载入中（载入中禁止重定位）
*/
void queryLoadMapStatus(QueryLoadMapStatusRes& res) override;
/**
* @brief 查询当前地图站点列表（命令码 1301, robot_status_station_req）
* @param res 输出参数，填充 QueryStationRes，包含所有站点信息
* @note 返回信息包括：站点 ID、类型、坐标、朝向角、属性等
*/
void queryStationList(QueryStationRes& res) override;
/**
* @brief 查询当前导航状态（命令码 1020, robot_status_task_req）
* @param res 输出参数，填充 RobotStatusTaskCurrentRes
* @param req 输入参数，simple=true 时只返回 task_status
* @note 返回信息包括：任务状态、任务类型、目标站点/坐标、已走/未走路径
*/
void robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& req) override;
/**
* @brief 批量查询任务状态（命令码 1110, robot_status_task_status_package_req）
* @param res 输出参数，填充 QueryTaskStatusPackageRes
* @param req 输入参数，task_ids 列表；若为空则查询所有未完成 + 最近一条已完成的任务
* @note 返回信息包括：任务列表、每个任务的 ID/状态/类型、进度百分比、剩余距离等
*/
void robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) override;
// ==================== 端口 19205 – 机器人控制 API（允许 5 个连接） ====================
// 功能：下发控制指令，改变机器人运动或状态（非导航类指令）
/**
* @brief 下发开环速度运动指令（命令码 2010, robot_control_motion_req）
* @param res 输出参数，执行结果
* @param req 输入参数，包含 vx/vy/w/steer/real_steer/duration
* @warning 下发此指令会立即取消当前正在执行的导航任务
* @note 多舵轮设备仅 vx/vy/w 生效，steer/real_steer 仅单舵轮设备有效
*/
void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) override;
/**
* @brief 切换载入地图（命令码 2022, robot_control_loadmap_req）
* @param res 输出参数，执行结果
* @param req 输入参数，目标地图名称
* @note 目标地图必须已存在于机器人中，否则切换失败
*/
void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) override;
// ==================== 端口 19206 – 机器人导航 API（允许 5 个连接） ====================
// 功能：下发导航任务
/**
* @brief 指定路径导航（命令码 3066, robot_task_gotargetlist_req）
* @param res 输出参数，下发结果（ret_code=0 仅表示指令被接收，不表示执行完成）
* @param req 输入参数，包含 move_task_list（站点序列）
* @attention
* - 每个任务必须含 task_id、source_id、id 三个必填字段
* - source_id 和 id 之间必须有直接相连的线路，不可跳点
* - 任务会排队执行，前一个任务失败时后续任务自动取消
* - 适合多车调度场景
*/
void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) override;
/**
* @brief 单点站点自动规划导航（命令码 3051, robot_task_gotarget_req）
* @param res 输出参数，下发结果（ret_code=0 仅表示指令被接收）
* @param req 输入参数，支持自由导航（freeGo）和基于站点的路径导航两种模式
* @warning
* - **严禁用于多车调度场景**，仅限单车测试/任务链验证
* - 下发新任务会取消当前正在执行的任务（不排队）
* - 成功下发后会自动清除指定的 warning/error 报错码
* - 自由导航（freeGo）仅支持双轮差速底盘
*/
void robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) override;
// ==================== 端口 19207 – 机器人配置 API（允许 5 个连接） ====================
// 功能：配置类操作（地图上传/下载、控制权管理、参数修改等）
/**
* @brief 抢占机器人控制权（命令码 4005, robot_config_lock_req）
* @param info 输出参数，填充 LockResult
* @param nick_name 抢占者昵称，用于标识调用方
* @note 抢占成功后调用方获得独占控制权，其他方仅可查询状态
*/
void lockRobotControl(LockResult& info, const std::string& nick_name) override;
/**
* @brief 上传地图（命令码 4010, robot_config_uploadmap_req）
* @param info 输出参数，填充 UploadMapResult
* @param map_json 地图 JSON 字符串（完整的地图文件内容）
* @note 地图数据较大时会自动分片发送，需确保 JSON 格式正确
*/
void uploadMap(UploadMapResult& info, const std::string& map_json) override;
/**
* @brief 下载指定地图（命令码 4011, robot_config_downloadmap_req）
* @param info 输出参数，填充 DownloadMapResult（包含地图 JSON 内容）
* @param map_name 要下载的地图名称
* @note 下载的地图内容以 JSON 字符串形式存储在 info.map_content 中
*/
void downloadMap(DownloadMapResult& info, const std::string& map_name) override;
// ==================== 端口 19210 – 其他 API（允许 5 个连接） ====================
// 功能：外设控制（顶升/货叉/辊筒/音频/IO 等）
// 当前未实现具体方法，预留扩展
// ==================== 端口 19301 – 机器人推送 API（允许 10 个连接） ====================
// 功能：接收机器人主动推送的实时状态数据
// 相关配置方法（如 9300）可能在此，当前未实现
// ---------- 新增任务控制接口（3001/3002/3003）及停止运动（2000） ----------
void robotTaskPause(RobotTaskPauseRes& res) override;
void robotTaskResume(RobotTaskResumeRes& res) override;
void robotTaskCancel(RobotTaskCancelRes& res) override;
void robotControlStop(RobotControlStopRes& res) override;
private:
// ==================== 私有通信辅助函数 ====================
/**
* @brief 异步连接指定端口
* @param sock 输出参数，连接成功后存储 socket 文件描述符
* @param port 目标端口号（19204/19205/19206/19207/19210/19301）
* @note 在独立线程中执行，连接成功后将 sock 设置为有效值
*/
void asyncConnect(int& sock, int port);
/**
* @brief 发送请求并接收完整响应
* @param sock 已连接的 socket 文件描述符
* @param header 16 字节协议帧头
* @return true 表示收发成功，current_json_ 中存储响应 JSON；false 表示失败
* @note 内部先发送帧头，然后循环接收直到超时，最后提取 JSON 部分
*/
bool sendAndRecv(int sock, const uint8_t* header);
/**
* @brief 清空 socket 接收缓冲区中的残留数据
* @param sock 目标 socket 文件描述符
* @note 采用非阻塞方式读取并丢弃所有可读数据，防止粘包干扰
*/
void flushSocket(int sock);
/**
* @brief 设置 socket 接收超时时间
* @param sock 目标 socket 文件描述符
* @param timeout_ms 超时时间（毫秒），0 表示取消超时
*/
void setSocketTimeout(int sock, int timeout_ms);
/**
* @brief 解析 JSON 字符串并做异常捕获
* @param json_str 输入的 JSON 字符串
* @param root 输出参数，解析后的 json 对象
* @param err_msg 输出参数，解析失败时的错误信息
* @return true 表示解析成功，false 表示解析失败
*/
bool parseJson(const std::string& json_str, json& root, std::string& err_msg);
// ==================== 成员变量 ====================
mutable std::mutex mutex_; ///< 保护状态和错误信息的互斥锁
std::atomic<Status> status_{Status::CREATED}; ///< 当前设备运行状态（原子变量）
std::string last_error_; ///< 最后一次错误信息
std::string current_json_; ///< 最近一次响应的 JSON 内容（由 sendAndRecv 填充）
// ---------- 各端口 Socket 文件描述符 ----------
int sock_status_ = -1; ///< 19204 – 状态 API 端口
int sock_control_ = -1; ///< 19205 – 控制 API 端口
int sock_nav_ = -1; ///< 19206 – 导航 API 端口
int sock_config_ = -1; ///< 19207 – 配置 API 端口
int sock_other_ = -1; ///< 19210 – 其他 API 端口
int sock_push_ = -1; ///< 19301 – 推送 API 端口
std::vector<std::thread> connect_threads_; ///< 各端口异步连接线程
mutable std::mutex connect_mutex_; ///< 保护 connect_threads_ 的互斥锁
};
} // namespace cmvr::device
#endif // AGV_SRC1100_H

2771
cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp
View File

File diff suppressed because it is too large Load Diff

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

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

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

@ -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,7 +18,6 @@ namespace cmvr::device {
class BioHeadRobot : public AbstractBiohead {
public:
explicit BioHeadRobot(const XmlNode &config);
BioHeadRobot(const cmvr::config::BioHeadConfig& config);
~BioHeadRobot() override = default;
// 接口实现
@ -67,7 +66,7 @@ namespace cmvr::device {
std::shared_ptr<std::thread> speak_thread_;
std::atomic<bool> speak_running_{false};
cmvr::config::BioHeadConfig config_;
};

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

@ -26,219 +26,6 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) {
}
BioHeadRobot::BioHeadRobot(const cmvr::config::BioHeadConfig& config) : config_(config) {
// 从config中读取串口参数
port_name_ = config_.serial();
serial_ = std::make_shared<SerialPort>();
if (!serial_->open(port_name_, 115200)) {
LOG(ERROR) << "[BioHeadRobot] Failed to open serial port: " << port_name_;
} else {
serial_->wakeupESP32(port_name_);
LOG(INFO) << "[BioHeadRobot] Serial port opened and ESP32 woken up.";
}
// 解析眉毛配置
if (config_.has_eyebrow()) {
const auto& eyebrow = config_.eyebrow();
std::string serial_str = eyebrow.serial();
std::string offset_str = eyebrow.offest();
std::string min_angles_str = eyebrow.jlmtlow();
std::string max_angles_str = eyebrow.jlmtup();
// 解析偏移角
std::vector<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> min_angles;
if (!min_angles_str.empty()) {
std::istringstream min_ss(min_angles_str);
while (min_ss >> val) min_angles.push_back(val);
}
std::vector<double> max_angles;
if (!max_angles_str.empty()) {
std::istringstream max_ss(max_angles_str);
while (max_ss >> val) max_angles.push_back(val);
}
// 解析 channel，如 64:0~3
size_t colon = serial_str.find(':');
if (colon != std::string::npos) {
uint8_t addr = std::stoi(serial_str.substr(0, colon));
std::string range = serial_str.substr(colon + 1);
size_t tilde = range.find('~');
if (tilde != std::string::npos) {
int ch_start = std::stoi(range.substr(0, tilde));
int ch_end = std::stoi(range.substr(tilde + 1));
int channel_count = ch_end - ch_start + 1;
// 添加舵机通道
for (int ch = ch_start; ch <= ch_end; ++ch) {
channels_.push_back({addr, static_cast<uint8_t>(ch)});
}
// 添加角度限制
if (min_angles.size() == 1) {
min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
} else if (min_angles.size() >= channel_count) {
min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
} else {
min_angles_.insert(min_angles_.end(), channel_count, 0.0);
}
if (max_angles.size() == 1) {
max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
} else if (max_angles.size() >= channel_count) {
max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
} else {
max_angles_.insert(max_angles_.end(), channel_count, 180.0);
}
}
}
}
// 解析眼睛配置
if (config_.has_eye()) {
const auto& eye = config_.eye();
std::string serial_str = eye.serial();
std::string offset_str = eye.offest();
std::string min_angles_str = eye.jlmtlow();
std::string max_angles_str = eye.jlmtup();
// 解析偏移角
std::vector<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> min_angles;
if (!min_angles_str.empty()) {
std::istringstream min_ss(min_angles_str);
while (min_ss >> val) min_angles.push_back(val);
}
std::vector<double> max_angles;
if (!max_angles_str.empty()) {
std::istringstream max_ss(max_angles_str);
while (max_ss >> val) max_angles.push_back(val);
}
// 解析 channel，如 64:4~9
size_t colon = serial_str.find(':');
if (colon != std::string::npos) {
uint8_t addr = std::stoi(serial_str.substr(0, colon));
std::string range = serial_str.substr(colon + 1);
size_t tilde = range.find('~');
if (tilde != std::string::npos) {
int ch_start = std::stoi(range.substr(0, tilde));
int ch_end = std::stoi(range.substr(tilde + 1));
int channel_count = ch_end - ch_start + 1;
// 添加舵机通道
for (int ch = ch_start; ch <= ch_end; ++ch) {
channels_.push_back({addr, static_cast<uint8_t>(ch)});
}
// 添加角度限制
if (min_angles.size() == 1) {
min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
} else if (min_angles.size() >= channel_count) {
min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
} else {
min_angles_.insert(min_angles_.end(), channel_count, 0.0);
}
if (max_angles.size() == 1) {
max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
} else if (max_angles.size() >= channel_count) {
max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
} else {
max_angles_.insert(max_angles_.end(), channel_count, 180.0);
}
}
}
}
// 解析嘴巴配置
if (config_.has_mouth()) {
const auto& mouth = config_.mouth();
std::string serial_str = mouth.serial();
std::string offset_str = mouth.offest();
std::string min_angles_str = mouth.jlmtlow();
std::string max_angles_str = mouth.jlmtup();
// 解析偏移角
std::vector<double> offsets;
std::istringstream oss(offset_str);
double val;
while (oss >> val) offsets.push_back(val);
// 解析角度限制
std::vector<double> min_angles;
if (!min_angles_str.empty()) {
std::istringstream min_ss(min_angles_str);
while (min_ss >> val) min_angles.push_back(val);
}
std::vector<double> max_angles;
if (!max_angles_str.empty()) {
std::istringstream max_ss(max_angles_str);
while (max_ss >> val) max_angles.push_back(val);
}
// 解析 channel，如 65:0~9
size_t colon = serial_str.find(':');
if (colon != std::string::npos) {
uint8_t addr = std::stoi(serial_str.substr(0, colon));
std::string range = serial_str.substr(colon + 1);
size_t tilde = range.find('~');
if (tilde != std::string::npos) {
int ch_start = std::stoi(range.substr(0, tilde));
int ch_end = std::stoi(range.substr(tilde + 1));
int channel_count = ch_end - ch_start + 1;
// 添加舵机通道
for (int ch = ch_start; ch <= ch_end; ++ch) {
channels_.push_back({addr, static_cast<uint8_t>(ch)});
}
// 添加角度限制
if (min_angles.size() == 1) {
min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]);
} else if (min_angles.size() >= channel_count) {
min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count);
} else {
min_angles_.insert(min_angles_.end(), channel_count, 0.0);
}
if (max_angles.size() == 1) {
max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]);
} else if (max_angles.size() >= channel_count) {
max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count);
} else {
max_angles_.insert(max_angles_.end(), channel_count, 180.0);
}
}
}
}
// 确保所有向量大小一致
if (min_angles_.size() != channels_.size()) {
min_angles_.resize(channels_.size(), 0.0);
}
if (max_angles_.size() != channels_.size()) {
max_angles_.resize(channels_.size(), 180.0);
}
current_joints_.resize(channels_.size(), 90.0);
last_joints_ = current_joints_;
}
void BioHeadRobot::parseXmlConfig(const XmlNode &node) {

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

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

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

@ -35,17 +35,6 @@ namespace cmvr {
// LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what();
}
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()) {

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

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

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

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

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

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

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

@ -1,21 +0,0 @@
add_library(eyoumotor SHARED
${CMAKE_CURRENT_SOURCE_DIR}/ethercat/protocol/ec_xml_parser.cpp
${CMAKE_CURRENT_SOURCE_DIR}/ethercat/protocol/ec_master.cpp
${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
-lsoem
protobuf
glog
)
install(TARGETS eyoumotor LIBRARY DESTINATION lib)

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

@ -1,747 +0,0 @@
//
// Created by cmvr on 2026/3/30.
//
#include "eyou_motor_ethercat_protocol.h"
#include "canbus/canopen/register.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
#include "canbus/canopen/nmt_response_protocol.h"
#include <cmath>
#include <thread>
#include <chrono>
using namespace cmvr::device;
// 辅助函数：获取操作模式字符串
static const char* getOpModeString(int8_t mode) {
switch (mode) {
case 1: return "Profile Position (PP)";
case 2: return "Velocity (VL)";
case 3: return "Profile Velocity (PV)";
case 4: return "Profile Torque (PT)";
case 6: return "Homing (HM)";
case 8: return "Cyclic Sync Position (CSP)";
case 9: return "Cyclic Sync Velocity (CSV)";
case 10: return "Cyclic Sync Torque (CST)";
default: return "Unknown";
}
}
// 构造函数
EYouEthercatProtocol::EYouEthercatProtocol(std::string ifName, const std::string& xmlPath)
: interface_name_(ifName),
xml_config_path_(xmlPath),
initialized_(false),
slave_count_(0),
use_fixed_pdo_(xmlPath.empty()) {
comm_proto = CommProto::ETHERCAT;
try {
// 初始化message_manager
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail>>();
// 创建 EcMaster 实例
master_ = std::make_unique<EcMaster>();
// 初始化主站
int cycleTimeUs = 1000; // 1ms周期
int slaveCount = master_->init(interface_name_.c_str(), cycleTimeUs);
if (slaveCount < 0) {
throw std::runtime_error("[EYouEthercatProtocol] master init failed");
}
slave_count_ = slaveCount;
EcSlaveConfig config = createFixedSlaveConfig(1);
std::vector<uint16_t> rxPdos = config.availableRxPdos;
std::vector<uint16_t> txPdos = config.availableTxPdos;
if (rxPdos.empty() || txPdos.empty()) {
LOG(ERROR) << "[EYouEthercatProtocol] No available PDOs found\n";
throw std::runtime_error("[EYouEthercatProtocol] No available PDOs found ");
}
const EcPdo* rxPdo = &config.modules[0].rxPdos[0];
const EcPdo* txPdo = &config.modules[0].txPdos[0];
for (int slaveId = 1; slaveId <= slave_count_; slaveId++) {
if (master_->configurePdo(slaveId, rxPdo, txPdo) != 0) {
throw std::runtime_error("[EYouEthercatProtocol] PDO configuration failed for slave " + std::to_string(slaveId));
}
}
master_->setupMappingAndDc();
// 等待配置生效
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// 进入OP状态
if (master_->gotoOperational() != 0) {
throw std::runtime_error("[EYouEthercatProtocol] OPERATIONAL state failed");
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
if (!master_->isOperational()) {
LOG(WARNING) << "[EYouEthercatProtocol] Master reports not operational";
} else {
LOG(INFO) << "[EYouEthercatProtocol] EtherCAT in OPERATIONAL state";
}
// 初始化从站状态
for (int i = 1; i <= slave_count_; i++) {
node_states_[i] = NodeState();
uint16_t status = master_->getStatusWord(i);
LOG(INFO) << "[EYouEthercatProtocol] Slave " << i << " initial status word: 0x" << std::hex << status << std::dec;
}
initialized_ = true;
LOG(INFO) << "[EYouEthercatProtocol] Initialized successfully";
// 添加NMT响应协议
for (int i = 1; i <= slave_count_; i++) {
message_manager_->AddRecvProtocolData<NmtResponseProtocol<cmvr::msgs::RobotDetail>, false>(i);
}
} catch (const std::exception& e) {
LOG(ERROR) << "[EYouEthercatProtocol] Constructor failed: " << e.what();
throw std::runtime_error(e.what());
}
}
// 析构函数
EYouEthercatProtocol::~EYouEthercatProtocol() {
if (master_) {
// 失能所有电机
for (auto& node : node_states_) {
if (node.second.enabled) {
master_->disable(node.first);
LOG(INFO) << "[EYouEthercatProtocol] Disabled motor " << node.first;
}
}
master_->shutdown();
LOG(INFO) << "[EYouEthercatProtocol] Master shutdown successfully";
}
}
// 执行电机使能序列
bool EYouEthercatProtocol::performEnableSequence(uint8_t node_id) {
if (node_id > slave_count_) {
LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " out of range";
return false;
}
// 读取状态字
uint16_t status = master_->getStatusWord(node_id);
LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " status: 0x" << std::hex << status << std::dec;
// 如果处于故障状态，先复位
if (status & 0x0008) {
LOG(INFO) << "[EYouEthercatProtocol] Fault detected, resetting...";
master_->faultReset(node_id);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
status = master_->getStatusWord(node_id);
}
// 如果处于 Switch On Disabled 状态，执行使能序列
if (status & 0x0040) {
LOG(INFO) << "[EYouEthercatProtocol] Node " << (int)node_id << " in Switch On Disabled, enabling...";
// 获取当前位置并同步目标位置（防止抖动）
int32_t actualPos = master_->getActualPosition(node_id);
master_->setTargetPosition(node_id, actualPos);
master_->setModeOfOperation(node_id,EcMaster::EcOperationMode_ProfilePosition);
// Shutdown
master_->setControlWord(node_id, 0x06);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Switch On
master_->setControlWord(node_id, 0x07);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Enable Operation
master_->setControlWord(node_id, 0x0F);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
status = master_->getStatusWord(node_id);
}
// 验证使能成功
bool enabled = (status & 0x0004) != 0;
if (enabled) {
LOG(INFO) << "[EYouEthercatProtocol] Node " << node_id << " enabled successfully";
} else {
LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " enable failed, status: 0x"
<< std::hex << status << std::dec;
}
return enabled;
}
// 操作模式转换 (msgs::RunMode -> EcOperationMode)
int8_t EYouEthercatProtocol::runModeToEcMode(msgs::RunMode mode) {
switch (mode) {
case msgs::RUN_MODE_PROFILE_POSITION:
return EcMaster::EcOperationMode_ProfilePosition; // 1
case msgs::RUN_MODE_PROFILE_VELOCITY:
return EcMaster::EcOperationMode_ProfileVelocity; // 3
case msgs::RUN_MODE_TORQUE: // 需要确认是否有这个枚举
return EcMaster::EcOperationMode_ProfileTorque; // 4
case msgs::RUN_MODE_INTERPOLATED_POSITION:
return EcMaster::EcOperationMode_InterpolatedPosition; // 7
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
return EcMaster::EcOperationMode_CyclicSyncPosition; // 8
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
return EcMaster::EcOperationMode_CyclicSyncVelocity; // 9
case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: // 名称统一
return EcMaster::EcOperationMode_CyclicSyncTorque; // 10
default:
return 0;
}
}
// 操作模式转换 (EcOperationMode -> msgs::RunMode)
cmvr::msgs::RunMode EYouEthercatProtocol::ecModeToRunMode(int8_t mode) {
switch (mode) {
case EcMaster::EcOperationMode_ProfilePosition:
return msgs::RUN_MODE_PROFILE_POSITION;
case EcMaster::EcOperationMode_ProfileVelocity:
return msgs::RUN_MODE_PROFILE_VELOCITY;
case EcMaster::EcOperationMode_ProfileTorque:
return msgs::RUN_MODE_TORQUE; // 需要确认枚举存在
case EcMaster::EcOperationMode_InterpolatedPosition:
return msgs::RUN_MODE_INTERPOLATED_POSITION;
case EcMaster::EcOperationMode_CyclicSyncPosition:
return msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
case EcMaster::EcOperationMode_CyclicSyncVelocity:
return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
case EcMaster::EcOperationMode_CyclicSyncTorque:
return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT;
default:
return msgs::RUN_MODE_UNSPECIFIED;
}
}
// 初始化节点
bool EYouEthercatProtocol::initNode(uint8_t node_id) {
try {
if (node_id > slave_count_) {
LOG(ERROR) << "[EYouEthercatProtocol] Node " << node_id << " out of range (max: " << slave_count_ << ")";
return false;
}
// 执行使能序列
if (!performEnableSequence(node_id)) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable node " << node_id;
return false;
}
node_states_[node_id].initialized = true;
node_states_[node_id].enabled = true;
node_states_[node_id].current_mode = msgs::RUN_MODE_PROFILE_POSITION;
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, msgs::RunMode mode) {
int8_t ec_mode = runModeToEcMode(mode);
master_->setModeOfOperation(node_id, ec_mode);
// 验证模式设置
std::this_thread::sleep_for(std::chrono::milliseconds(20));
int8_t actual_mode = master_->getModeOfOperationDisplay(node_id);
if (actual_mode == ec_mode) {
node_states_[node_id].current_mode = mode;
LOG(INFO) << "[EYouEthercatProtocol] Node " << (int)node_id << " mode set to "
<< getOpModeString(ec_mode);
} else {
LOG(WARNING) << "[EYouEthercatProtocol] Node " << node_id << " mode set failed, "
<< "expected " << getOpModeString(ec_mode)
<< ", got " << getOpModeString(actual_mode);
}
}
// 设置目标位置和速度
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 msgs::RUN_MODE_PROFILE_POSITION: {
// 先设置轮廓速度
master_->setProfileVelocity(node_id, speed);
// 设置目标位置
master_->setTargetPosition(node_id, pos_cmd);
node_states_[node_id].target_position = pos_cmd;
// 构建控制字：使能状态 + 立即更新标志
uint16_t ctlword = 0x0F | 0x20; // 0x0F使能, 0x20立即更新
// 触发位置更新
master_->setControlWord(node_id, ctlword);
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// 启动新位置命令
master_->setControlWord(node_id, ctlword | 0x10);
break;
}
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
master_->setTargetPosition(node_id, pos_cmd);
node_states_[node_id].target_position = 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 msgs::RUN_MODE_VELOCITY:
case msgs::RUN_MODE_PROFILE_VELOCITY:
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
master_->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 msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
{
master_->setTargetPosition(node_id, pos_cmd);
node_states_[node_id].target_position = pos_cmd;
break;
}
case msgs::RUN_MODE_PROFILE_POSITION:
{
// 1. 设置目标位置
master_->setTargetPosition(node_id, pos_cmd);
node_states_[node_id].target_position = pos_cmd;
// 2. 构建控制字：使能状态 + 立即更新标志
uint16_t ctlword = 0x0F | 0x20; // 0x0F使能, 0x20立即更新
// 3. 触发位置更新
master_->setControlWord(node_id, ctlword);
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// 4. 启动新位置命令
master_->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);
// 开启软件位置限制 (0x4C494D54 = "LIMT")
uint32_t limitEnable = 0x4C494D54;
if (master_->writeSdo(node_id, 0x2003, 0x00, &limitEnable, EcDataType::Uint32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to enable software position limit for node " << node_id;
}
// 设置软件位置限制
if (master_->writeSdo(node_id, 0x607D, 0x01, &lb_val, EcDataType::Int32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set lower position limit for node " << node_id;
}
if (master_->writeSdo(node_id, 0x607D, 0x02, &ub_val, EcDataType::Int32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set upper position limit for node " << node_id;
}
LOG(INFO) << "[EYouEthercatProtocol] Position limits set for node " << node_id
<< ": [" << lb << ", " << ub << "] rad";
}
// 设置速度限制
void EYouEthercatProtocol::setLimitQd(uint8_t node_id, double qd) {
auto speed = static_cast<uint32_t>(std::abs(velToSpeed(qd)));
// 设置最大速度限制
if (master_->writeSdo(node_id, 0x607F, 0x00, &speed, EcDataType::Uint32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set max speed limit for node " << node_id;
}
if (master_->writeSdo(node_id, 0x6081, 0x00, &speed, EcDataType::Uint32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set profile speed for node " << node_id;
}
LOG(INFO) << "[EYouEthercatProtocol] Speed limit set for node " << node_id << ": " << qd << " rad/s";
}
// 设置加速度限制
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 (master_->writeSdo(node_id, 0x6083, 0x00, &accel, EcDataType::Uint32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set acceleration limit for node " << node_id;
}
if (master_->writeSdo(node_id, 0x6084, 0x00, &decel, EcDataType::Uint32, 100) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] Failed to set deceleration limit for node " << node_id;
}
LOG(INFO) << "[EYouEthercatProtocol] Acceleration limits set for node " << node_id
<< ": accel=" << u_qdd << ", decel=" << l_qdd << " rad/s²";
}
// 零位校准
bool EYouEthercatProtocol::calibrateZeroQ(uint8_t node_id) {
try {
int32_t pos;
int32_t offset = 0;
int32_t rev = 0;
// 读取当前位置
if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] read actual position failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Actual position: " << pos;
// 读取home offset
if (master_->readSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] read home offset failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Home offset before: " << offset;
// 先将home offset设为0
offset = 0;
if (master_->writeSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] write home offset 0 failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Home offset set to 0";
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// 读取实际位置
if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] read actual position after offset reset failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Actual position after offset reset: " << pos;
// 获取轴转数，超圈处理
if (master_->readSdo(node_id, 0x6091, 0x02, &rev, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] read gear ratio shaft revolution failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Gear ratio shaft revolution: " << rev;
// 处理超出编码器范围的情况
long long r = rev;
int temp = static_cast<int>(r / 2);
if (pos > temp || pos < -temp) {
pos = pos % static_cast<int>(r);
if (pos > temp || pos < -temp) {
if (pos > temp) {
pos = pos - static_cast<int>(r);
} else if (pos < -temp) {
pos = pos + static_cast<int>(r);
}
}
LOG(WARNING) << "[EYouEthercatProtocol] Position out of encoder range, adjusted to " << pos;
}
// 写home offset偏移
offset = -pos;
if (master_->writeSdo(node_id, 0x607C, 0x00, &offset, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] write home offset failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Home offset set to: " << offset;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// 读取实际位置
if (master_->readSdo(node_id, 0x6064, 0x00, &pos, EcDataType::Int32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] read actual position after calibration failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Actual position after calibration: " << pos;
// 保存参数
uint32_t saveValue = 0x65766173; // "evas"
if (master_->writeSdo(node_id, 0x1010, 0x01, &saveValue, EcDataType::Uint32, 20000) != 0) {
LOG(ERROR) << "[EYouEthercatProtocol] save parameters failed";
return false;
}
LOG(INFO) << "[EYouEthercatProtocol] Parameters saved";
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) {
// 发送快速停机命令
uint16_t ctlword = 0x0002; // Quick Stop
master_->setControlWord(node_id, ctlword);
LOG(INFO) << "[EYouEthercatProtocol] Brake (quick stop) applied to motor " << node_id;
}
// 检查是否到达目标位置
bool EYouEthercatProtocol::reachedTargetQ(uint8_t node_id) {
uint16_t status_word = getStatusWord(node_id);
// 检查状态字的bit 10（目标到达位）
return (status_word & 0x0400) != 0;
}
// 获取当前位置
double EYouEthercatProtocol::getQ(uint8_t node_id) {
int32_t position = master_->getActualPosition(node_id);
return posToAngle(position);
}
// 获取当前速度
double EYouEthercatProtocol::getQd(uint8_t node_id) {
int32_t velocity = master_->getActualVelocity(node_id);
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 msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION:
master_->setProfileVelocity(node_id, speed);
last_Qd_[node_id] = speed;
break;
case msgs::RUN_MODE_PROFILE_VELOCITY:
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
master_->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² (加速度)
// 转换为电机加速度（脉冲/秒²）
uint32_t accel = static_cast<uint32_t>((std::abs(qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV);
if (last_Qdd_[node_id] != accel) {
master_->setProfileAcceleration(node_id, accel);
master_->setProfileDeceleration(node_id, accel);
last_Qdd_[node_id] = accel;
}
}
// 力矩关闭
void EYouEthercatProtocol::torqueOff(uint8_t node_id) {
auto ret = master_->disable(node_id);
node_states_[node_id].enabled = false;
LOG(INFO) << "[EYouEthercatProtocol] Torque off for motor " << node_id << ", ret = " << ret;
}
// 获取运行模式
cmvr::msgs::RunMode EYouEthercatProtocol::getMode(uint8_t node_id) {
int8_t mode = master_->getModeOfOperationDisplay(node_id);
return ecModeToRunMode(mode);
}
// 配置轮廓参数
void EYouEthercatProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
master_->setProfileVelocity(node_id, speed);
master_->setProfileAcceleration(node_id, accel);
master_->setProfileDeceleration(node_id, decel);
LOG(INFO) << "[EYouEthercatProtocol] Profile configured for node " << node_id
<< ": speed=" << speed << ", accel=" << accel << ", decel=" << decel;
}
// 获取力矩
double EYouEthercatProtocol::getTorque(uint8_t node_id) {
int16_t torque = master_->getActualTorque(node_id);
// 返回实际力矩值（取决于具体驱动器，这里假设是额定力矩的倍数）
return torque / 1000.0;
}
// 获取状态字
uint16_t EYouEthercatProtocol::getStatusWord(uint8_t node_id) {
uint16_t status_word = master_->getStatusWord(node_id);
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;
}
// 故障复位
void EYouEthercatProtocol::faultReset(uint8_t node_id) {
if (isFault(node_id)) {
if (master_->faultReset(node_id) == 0) {
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<cmvr::msgs::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) {
// 更新状态字
node_states_[node_id].status_word = master_->getStatusWord(node_id);
// 可以根据需要更新其他状态信息
}

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

@ -1,125 +0,0 @@
//
// Created by cmvr on 2026/3/30.
//
#pragma once
#include "cmvr/msgs/motor.pb.h"
#include "motor/motor_protocol_interface.h"
#include "../../../canbus/abstract_canbus.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/error_code.pb.h"
#include "cmvr/msgs/robot_detail.pb.h"
// 替换原有的 eu_ethercat 头文件
#include "protocol/ec_master.h"
#include "protocol/ec_xml_parser.h"
#include "protocol/ec_pdo_config.h"
#include <cmath>
#include <unordered_map>
#include <functional>
#include <memory>
#include <chrono>
#include <thread>
#include <mutex>
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;
int32_t target_position = 0;
};
class EYouEthercatProtocol : public MotorProtocolInterface {
public:
explicit EYouEthercatProtocol(std::string ifName, const std::string& xmlPath = "");
~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();
int getSlaveCount() const { return slave_count_; }
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);
bool performEnableSequence(uint8_t node_id);
int8_t runModeToEcMode(msgs::RunMode mode);
msgs::RunMode ecModeToRunMode(int8_t mode);
// 成员变量
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_;
std::string xml_config_path_;
bool initialized_;
int slave_count_;
std::unique_ptr<EcMaster> master_;
std::unique_ptr<EcXmlParser> parser_;
EcSlaveConfig slave_config_;
bool use_fixed_pdo_;
std::mutex master_mutex_;
};
} // namespace device
} // namespace cmvr

1143
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp
View File

File diff suppressed because it is too large Load Diff

328
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h
View File

@ -1,328 +0,0 @@
// ec_master.h
#ifndef EC_MASTER_H
#define EC_MASTER_H
#include "ec_types.h"
#include <map>
#include <atomic>
#include <cstring>
#include <vector>
#ifdef __cplusplus
extern "C" {
#endif
#include "soem/soem.h"
#include "soem/osal.h"
#ifdef __cplusplus
}
#endif
// 错误码定义
#define EC_SUCCESS 0
#define EC_FAILED_INIT 1
#define EC_FAILED_NOSLAVE 2
#ifndef EC_TIMEOUTMON
#define EC_TIMEOUTMON 500
#endif
#ifndef EC_TIMEOUTRET
#define EC_TIMEOUTRET 2000
#endif
#ifndef EC_TIMEOUTSTATE
#define EC_TIMEOUTSTATE 2000
#endif
#ifndef EC_TIMEOUTRXM
#define EC_TIMEOUTRXM 2000
#endif
// EtherCAT 主站类
class EcMaster {
public:
// 操作模式枚举（符合 CiA 402 标准）
enum EcOperationMode : int8_t {
// 轮廓模式 (Profile Modes)
EcOperationMode_ProfilePosition = 1, // PP 模式
EcOperationMode_ProfileVelocity = 3, // PV 模式
EcOperationMode_ProfileTorque = 4, // PT 模式
// 插补模式
EcOperationMode_InterpolatedPosition = 7, // IP 模式
// 循环同步模式 (Cyclic Sync Modes)
EcOperationMode_CyclicSyncPosition = 8, // CSP 模式
EcOperationMode_CyclicSyncVelocity = 9, // CSV 模式
EcOperationMode_CyclicSyncTorque = 10, // CST 模式
};
public:
EcMaster();
~EcMaster();
EcMaster(const EcMaster&) = delete;
EcMaster& operator=(const EcMaster&) = delete;
// 初始化
int init(const char* ifName, int cycleTimeUs = 1000);
void close() { shutdown(); }
int getSlaveCount() const { return slaveCount; }
bool isOperational() const { return inOp; }
// 状态控制
int setState(uint16_t slaveId, uint8_t targetState);
int preOperational(uint16_t slaveId) { return setState(slaveId, EC_STATE_PRE_OP); }
int safeOperational(uint16_t slaveId) { return setState(slaveId, EC_STATE_SAFE_OP); }
int operational(uint16_t slaveId) { return setState(slaveId, EC_STATE_OPERATIONAL); }
int gotoOperational();
void shutdown();
// PDO 配置
int configurePdo(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo);
int setupMappingAndDc(); // 配置所有从站的映射和DC
int setupPdoOffsets(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo);
// PDO 数据访问（高性能，零拷贝）
void setControlWord(uint16_t slaveId, uint16_t value);
uint16_t getStatusWord(uint16_t slaveId);
void setTargetPosition(uint16_t slaveId, int32_t value);
int32_t getActualPosition(uint16_t slaveId);
void setTargetVelocity(uint16_t slaveId, int32_t value);
int32_t getActualVelocity(uint16_t slaveId);
void setTargetTorque(uint16_t slaveId, int16_t value);
int16_t getActualTorque(uint16_t slaveId);
void setModeOfOperation(uint16_t slaveId, int8_t value);
int8_t getModeOfOperationDisplay(uint16_t slaveId);
void setProfileVelocity(uint16_t slaveId, uint32_t value);
void setProfileAcceleration(uint16_t slaveId, uint32_t value);
void setProfileDeceleration(uint16_t slaveId, uint32_t value);
int16_t getCurrentValue(uint16_t slaveId);
uint16_t getErrorCode(uint16_t slaveId);
// 电机控制高级接口
int enable(uint16_t slaveId);
int disable(uint16_t slaveId);
int faultReset(uint16_t slaveId);
// 周期操作
int sendOutputs() { return ecx_send_processdata(&ctx); }
int receiveInputs() { return ecx_receive_processdata(&ctx, EC_TIMEOUTRET); }
// 影子缓冲区同步接口
void syncSlaveOutputs(uint16_t slaveId); // 同步指定从站的输出
void syncAllOutputs(); // 同步所有从站的输出
// 原始数据访问
uint8_t* getInputs(uint16_t slaveId) {
auto it = inputs.find(slaveId);
return (it != inputs.end()) ? it->second : nullptr;
}
uint8_t* getOutputs(uint16_t slaveId) {
auto it = outputs.find(slaveId);
return (it != outputs.end()) ? it->second : nullptr;
}
int getInputSize(uint16_t slaveId) {
auto it = inputSizes.find(slaveId);
return (it != inputSizes.end()) ? it->second : 0;
}
int getOutputSize(uint16_t slaveId) {
auto it = outputSizes.find(slaveId);
return (it != outputSizes.end()) ? it->second : 0;
}
// SDO 读写（仅用于配置阶段）
int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs = 100);
int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs = 100);
// SM/FMMU 配置校验（调试用）
void verifySmConfiguration(uint16_t slaveId);
void verifyAllSlavesSmConfiguration();
// 模板便捷接口
template<typename T>
int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) {
EcDataType type;
if constexpr (std::is_same_v<T, int8_t>) type = EcDataType::Int8;
else if constexpr (std::is_same_v<T, uint8_t>) type = EcDataType::Uint8;
else if constexpr (std::is_same_v<T, int16_t>) type = EcDataType::Int16;
else if constexpr (std::is_same_v<T, uint16_t>) type = EcDataType::Uint16;
else if constexpr (std::is_same_v<T, int32_t>) type = EcDataType::Int32;
else if constexpr (std::is_same_v<T, uint32_t>) type = EcDataType::Uint32;
else if constexpr (std::is_same_v<T, float>) type = EcDataType::Real32;
else if constexpr (std::is_same_v<T, double>) type = EcDataType::Real64;
else return -1;
return readSdo(slaveId, index, subindex, value, type, timeoutMs);
}
template<typename T>
int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) {
EcDataType type;
if constexpr (std::is_same_v<T, int8_t>) type = EcDataType::Int8;
else if constexpr (std::is_same_v<T, uint8_t>) type = EcDataType::Uint8;
else if constexpr (std::is_same_v<T, int16_t>) type = EcDataType::Int16;
else if constexpr (std::is_same_v<T, uint16_t>) type = EcDataType::Uint16;
else if constexpr (std::is_same_v<T, int32_t>) type = EcDataType::Int32;
else if constexpr (std::is_same_v<T, uint32_t>) type = EcDataType::Uint32;
else if constexpr (std::is_same_v<T, float>) type = EcDataType::Real32;
else if constexpr (std::is_same_v<T, double>) type = EcDataType::Real64;
else return -1;
return writeSdo(slaveId, index, subindex, value, type, timeoutMs);
}
private:
// ========== 影子缓冲区结构（每个从站独立） ==========
struct SlaveShadowBuffer {
uint8_t* data = nullptr; // 影子缓冲区数据指针
size_t size = 0; // 缓冲区大小
bool dirty = false; // 是否有待同步的数据
SlaveShadowBuffer() = default;
explicit SlaveShadowBuffer(size_t sz) : size(sz) {
if (sz > 0) {
data = new uint8_t[sz];
memset(data, 0, sz);
}
}
~SlaveShadowBuffer() {
delete[] data;
data = nullptr;
}
// 禁止拷贝
SlaveShadowBuffer(const SlaveShadowBuffer&) = delete;
SlaveShadowBuffer& operator=(const SlaveShadowBuffer&) = delete;
// 允许移动
SlaveShadowBuffer(SlaveShadowBuffer&& other) noexcept
: data(other.data), size(other.size), dirty(other.dirty) {
other.data = nullptr;
other.size = 0;
other.dirty = false;
}
SlaveShadowBuffer& operator=(SlaveShadowBuffer&& other) noexcept {
if (this != &other) {
delete[] data;
data = other.data;
size = other.size;
dirty = other.dirty;
other.data = nullptr;
other.size = 0;
other.dirty = false;
}
return *this;
}
// 重新分配缓冲区
void reallocate(size_t newSize) {
if (data) {
delete[] data;
data = nullptr;
}
size = newSize;
if (newSize > 0) {
data = new uint8_t[newSize];
memset(data, 0, newSize);
}
dirty = false;
}
};
// SOEM 上下文
ecx_contextt ctx;
uint8_t IOmap[4096];
// 每个从站的影子缓冲区映射
std::map<uint16_t, SlaveShadowBuffer> shadowBuffers;
std::atomic<bool> anyPdoDirty{false}; // 是否有任何影子缓冲区有更新
int slaveCount = 0;
int expectedWKC = 0;
int currentgroup = 0;
// 状态标志
std::atomic<bool> inOp{false};
std::atomic<bool> isRunning{false};
std::atomic<int> mappingdone{0};
std::atomic<int> dorun{0};
std::atomic<int> cyclicWkcErrorCount{0};
int64_t cycleTimeNs = 1000000;
OSAL_THREAD_HANDLE cyclicThread;
OSAL_THREAD_HANDLE checkThread;
// PDO 数据指针映射
std::map<uint16_t, uint8_t*> inputs;
std::map<uint16_t, uint8_t*> outputs;
std::map<uint16_t, int> inputSizes;
std::map<uint16_t, int> outputSizes;
std::map<uint16_t, uint16_t> outputOffsets;
// PDO 偏移映射
std::map<uint16_t, EcPdoOffsets> pdoOffsets;
// 辅助函数
template<typename T>
void writePdo(uint16_t slaveId, uint16_t offset, T value);
template<typename T>
T readPdo(uint16_t slaveId, uint16_t offset);
// 获取从站的影子缓冲区
SlaveShadowBuffer* getShadowBuffer(uint16_t slaveId);
// 线程函数
static OSAL_THREAD_FUNC_RT cyclicThreadFunc(void* arg);
static OSAL_THREAD_FUNC checkThreadFunc(void* arg);
void runCyclic();
void runCheck();
};
// ==================== 模板实现（必须放在头文件中） ====================
template<typename T>
void EcMaster::writePdo(uint16_t slaveId, uint16_t offset, T value) {
if (inOp) {
auto* shadow = getShadowBuffer(slaveId);
if (shadow && shadow->data) {
if (offset + sizeof(T) <= shadow->size) {
*(T*)(shadow->data + offset) = value;
shadow->dirty = true;
anyPdoDirty = true;
return;
}
}
}
// 降级：直接写 IOmap（配置阶段使用）
auto outIt = outputs.find(slaveId);
if (outIt != outputs.end() && outIt->second) {
auto sizeIt = outputSizes.find(slaveId);
if (sizeIt != outputSizes.end() && offset + sizeof(T) <= (uint16_t)sizeIt->second) {
*(T*)(outIt->second + offset) = value;
}
}
}
template<typename T>
T EcMaster::readPdo(uint16_t slaveId, uint16_t offset) {
auto it = pdoOffsets.find(slaveId);
if (it == pdoOffsets.end() || offset == 0xFFFF) return 0;
auto inIt = inputs.find(slaveId);
if (inIt != inputs.end() && inIt->second) {
auto sizeIt = inputSizes.find(slaveId);
if (sizeIt != inputSizes.end() && offset + sizeof(T) <= (uint16_t)sizeIt->second) {
return *(T*)(inIt->second + offset);
}
}
return 0;
}
#endif // EC_MASTER_H

75
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h
View File

@ -1,75 +0,0 @@
// ec_pdo_config.h - 固定 PDO 配置（替代 XML 解析）
#ifndef EC_PDO_CONFIG_H
#define EC_PDO_CONFIG_H
#include "ec_types.h"
// 创建固定的 RxPDO 配置（对应 XML 中的 #x1600）
inline EcPdo createFixedRxPdo() {
EcPdo rxPdo;
rxPdo.index = 0x1600;
rxPdo.mapCount = 10; // 9个有效条目 + 1个结束标志
rxPdo.name = "Outputs";
rxPdo.isRxPdo = true;
// 按XML顺序填充 Entry
rxPdo.entries.push_back({0x6040, 0, 16, "Control Word", EcDataType::Uint16});
rxPdo.entries.push_back({0x607A, 0, 32, "Target Position", EcDataType::Int32});
rxPdo.entries.push_back({0x60FF, 0, 32, "Target Velocity", EcDataType::Int32});
rxPdo.entries.push_back({0x6071, 0, 16, "Target Torque", EcDataType::Int16});
rxPdo.entries.push_back({0x6083, 0, 32, "Profile Acceleration", EcDataType::Uint32});
rxPdo.entries.push_back({0x6084, 0, 32, "Profile Deceleration", EcDataType::Uint32});
rxPdo.entries.push_back({0x6081, 0, 32, "Profile Velocity", EcDataType::Uint32});
rxPdo.entries.push_back({0x6087, 0, 32, "Torque Slope", EcDataType::Uint32});
rxPdo.entries.push_back({0x6060, 0, 8, "Modes Of Operation", EcDataType::Int8});
rxPdo.entries.push_back({0x0000, 0, 8, "End Marker", EcDataType::Uint8});
return rxPdo;
}
// 创建固定的 TxPDO 配置（对应 XML 中的 #x1a00）
inline EcPdo createFixedTxPdo() {
EcPdo txPdo;
txPdo.index = 0x1A00;
txPdo.mapCount = 8; // 7个有效条目 + 1个结束标志
txPdo.name = "Inputs";
txPdo.isRxPdo = false;
// 按XML顺序填充 Entry
txPdo.entries.push_back({0x6041, 0, 16, "Status Word", EcDataType::Uint16});
txPdo.entries.push_back({0x6064, 0, 32, "Actual Position", EcDataType::Int32});
txPdo.entries.push_back({0x606C, 0, 32, "Actual Velocity", EcDataType::Int32});
txPdo.entries.push_back({0x6077, 0, 16, "Actual Torque", EcDataType::Int16});
txPdo.entries.push_back({0x6061, 0, 8, "Mode Of Operation Display", EcDataType::Int8});
txPdo.entries.push_back({0x603F, 0, 16, "Error Code", EcDataType::Uint16});
txPdo.entries.push_back({0x6078, 0, 16, "Actual Current Value", EcDataType::Int16});
txPdo.entries.push_back({0x0000, 0, 8, "End Marker", EcDataType::Uint8});
return txPdo;
}
// 创建完整的从站配置 - 可以直接替换 XML 解析结果
inline EcSlaveConfig createFixedSlaveConfig(uint16_t slaveId) {
EcSlaveConfig config;
config.slaveId = slaveId;
config.productCode = 0x00000000;
config.revisionNo = 0;
config.name = "Axis (csv,csp,cst)";
EcModule module;
module.moduleIdent = 0x119800; // 对应 XML 中的 ModuleIdent="#x119800"
module.name = "dynamic switchbewteen csp/csv";
module.rxPdos.push_back(createFixedRxPdo());
module.txPdos.push_back(createFixedTxPdo());
// 记录可用的PDO索引（对应 parser.getAvailableRxPdos/getAvailableTxPdos 的返回值）
config.availableRxPdos.push_back(0x1600);
config.availableTxPdos.push_back(0x1A00);
config.modules.push_back(module);
return config;
}
#endif // EC_PDO_CONFIG_H

111
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h
View File

@ -1,111 +0,0 @@
// ec_types.h - 公共类型定义
#ifndef EC_TYPES_H
#define EC_TYPES_H
#include <string>
#include <vector>
#include <cstdint>
// 数据类型枚举
enum class EcDataType : uint8_t {
Boolean = 0x01,
Int8 = 0x02,
Int16 = 0x03,
Int32 = 0x04,
Uint8 = 0x05,
Uint16 = 0x06,
Uint32 = 0x07,
Real32 = 0x08,
Real64 = 0x09
};
// 从站状态枚举
enum class EcState : uint8_t {
Init = 0x01,
PreOp = 0x02,
SafeOp = 0x04,
Operational = 0x08
};
// 操作模式枚举
enum class EcOperateMode : int8_t {
ProfilePosition = 1,
Velocity = 2,
ProfileVelocity = 3,
ProfileTorque = 4,
Homing = 6,
CyclicSyncPosition = 8,
CyclicSyncVelocity = 9,
CyclicSyncTorque = 10
};
// PDO 条目
struct EcPdoEntry {
uint16_t index = 0;
uint8_t subindex = 0;
uint8_t bitlen = 0;
std::string name;
EcDataType dataType = EcDataType::Uint8;
uint32_t makeMapValue() const {
return ((uint32_t)index << 16) | ((uint32_t)subindex << 8) | bitlen;
}
};
// PDO
struct EcPdo {
uint16_t index = 0;
uint8_t mapCount = 0;
std::vector<EcPdoEntry> entries;
std::string name;
bool isRxPdo = false;
void updateMapCount() {
mapCount = entries.size();
}
};
// 模块信息
struct EcModule {
uint32_t moduleIdent = 0;
std::string name;
std::vector<EcPdo> rxPdos;
std::vector<EcPdo> txPdos;
};
// 从站配置信息
struct EcSlaveConfig {
uint16_t slaveId = 0;
uint32_t productCode = 0;
uint32_t revisionNo = 0;
std::string name;
std::vector<EcModule> modules;
std::vector<uint16_t> availableRxPdos;
std::vector<uint16_t> availableTxPdos;
};
// PDO 偏移映射
// ec_types.h 或 ec_master.h 中
struct EcPdoOffsets {
// RxPDO 偏移量 (主站 -> 从站)
uint16_t controlWord = 0xFFFF; // 0x6040
uint16_t targetPosition = 0xFFFF; // 0x607A
uint16_t targetVelocity = 0xFFFF; // 0x60FF
uint16_t targetTorque = 0xFFFF; // 0x6071
uint16_t profileAcceleration = 0xFFFF; // 0x6083 (PP模式)
uint16_t profileDeceleration = 0xFFFF; // 0x6084 (PP模式)
uint16_t profileVelocity = 0xFFFF; // 0x6081 (PP模式)
uint16_t torqueSlope = 0xFFFF; // 0x6087 (PT模式)
uint16_t modeOfOperation = 0xFFFF; // 0x6060
// TxPDO 偏移量 (从站 -> 主站)
uint16_t statusWord = 0xFFFF; // 0x6041
uint16_t actualPosition = 0xFFFF; // 0x6064
uint16_t actualVelocity = 0xFFFF; // 0x606C
uint16_t actualTorque = 0xFFFF; // 0x6077
uint16_t modeOfOperationDisplay = 0xFFFF;// 0x6061
uint16_t errorCode = 0xFFFF; // 0x603F
uint16_t actualCurrent = 0xFFFF; // 0x6078 当前电流值
};
#endif // EC_TYPES_H

317
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp
View File

@ -1,317 +0,0 @@
// ec_xml_parser.cpp
#include "ec_xml_parser.h"
#include "rapidxml/xml_parser.h"
#include <cstdlib>
#include <cstdio>
#include <map>
using namespace rapidxml;
static EcDataType stringToDataType(const std::string& typeName) {
if (typeName == "BOOL") return EcDataType::Boolean;
if (typeName == "SINT") return EcDataType::Int8;
if (typeName == "INT") return EcDataType::Int16;
if (typeName == "DINT") return EcDataType::Int32;
if (typeName == "USINT") return EcDataType::Uint8;
if (typeName == "UINT") return EcDataType::Uint16;
if (typeName == "UDINT") return EcDataType::Uint32;
if (typeName == "REAL") return EcDataType::Real32;
return EcDataType::Uint8;
}
static uint32_t parseHexValue(const std::string& str) {
if (str.empty()) return 0;
if (str[0] == '#' && str[1] == 'x') {
return strtoul(str.c_str() + 2, nullptr, 16);
}
return strtoul(str.c_str(), nullptr, 16);
}
class EcXmlParser::Impl {
public:
std::map<uint16_t, EcSlaveConfig> slaveConfigs;
std::unique_ptr<XmlNode> xmlDoc;
bool loadXmlFile(const std::string& path) {
try {
xmlDoc = std::make_unique<XmlNode>(path);
return true;
} catch (const std::exception& e) {
printf("Failed to load XML file: %s, error: %s\n", path.c_str(), e.what());
return false;
}
}
void parsePdoEntry(const XmlNode& entryNode, EcPdoEntry& entry) {
if (entryNode.hasChild("Index")) {
XmlNode idxNode = entryNode.getChild("Index");
entry.index = (uint16_t)parseHexValue(idxNode.getNodeValue());
}
if (entryNode.hasChild("SubIndex")) {
XmlNode subIdxNode = entryNode.getChild("SubIndex");
entry.subindex = (uint8_t)atoi(subIdxNode.getNodeValue().c_str());
}
if (entryNode.hasChild("BitLen")) {
XmlNode bitLenNode = entryNode.getChild("BitLen");
entry.bitlen = (uint8_t)atoi(bitLenNode.getNodeValue().c_str());
}
if (entryNode.hasChild("Name")) {
XmlNode nameEntryNode = entryNode.getChild("Name");
entry.name = nameEntryNode.getNodeValue();
}
if (entryNode.hasChild("DataType")) {
XmlNode dataTypeNode = entryNode.getChild("DataType");
entry.dataType = stringToDataType(dataTypeNode.getNodeValue());
}
}
void parsePdoMapping(const XmlNode& pdoNode, bool isRxPdo, EcModule& module) {
if (!pdoNode.hasChild("Index")) {
printf("Warning: PDO missing Index node, skipping\n");
return;
}
XmlNode indexNode = pdoNode.getChild("Index");
uint16_t index = (uint16_t)parseHexValue(indexNode.getNodeValue());
EcPdo pdo;
pdo.index = index;
pdo.isRxPdo = isRxPdo;
if (pdoNode.hasChild("Name")) {
XmlNode nameNode = pdoNode.getChild("Name");
pdo.name = nameNode.getNodeValue();
}
std::vector<XmlNode> entries = pdoNode.getChildren("Entry");
for (const auto& entryNode : entries) {
EcPdoEntry entry;
parsePdoEntry(entryNode, entry);
pdo.entries.push_back(entry);
}
pdo.updateMapCount();
if (isRxPdo) {
module.rxPdos.push_back(pdo);
} else {
module.txPdos.push_back(pdo);
}
}
void parseModule(const XmlNode& moduleNode, EcSlaveConfig& config) {
if (!moduleNode.hasChild("Type")) {
printf("Warning: Module missing Type node, skipping\n");
return;
}
auto typeNode = moduleNode.getChild("Type");
std::string moduleIdentStr = typeNode.getAttrDefault("ModuleIdent", "");
if (moduleIdentStr.empty()) {
printf("Warning: Module missing ModuleIdent attribute, skipping\n");
return;
}
EcModule module;
module.moduleIdent = (uint32_t)parseHexValue(moduleIdentStr);
if (moduleNode.hasChild("Name")) {
XmlNode nameNode = moduleNode.getChild("Name");
module.name = nameNode.getNodeValue();
}
if (moduleNode.hasChild("RxPdo")) {
auto rxPdoNode = moduleNode.getChild("RxPdo");
parsePdoMapping(rxPdoNode, true, module);
}
if (moduleNode.hasChild("TxPdo")) {
auto txPdoNode = moduleNode.getChild("TxPdo");
parsePdoMapping(txPdoNode, false, module);
}
config.modules.push_back(module);
for (const auto& pdo : module.rxPdos) {
config.availableRxPdos.push_back(pdo.index);
}
for (const auto& pdo : module.txPdos) {
config.availableTxPdos.push_back(pdo.index);
}
printf(" Parsed module: 0x%08X - %s\n", module.moduleIdent, module.name.c_str());
printf(" RxPDOs: %zu, TxPDOs: %zu\n", module.rxPdos.size(), module.txPdos.size());
}
bool parseSlaveConfigById(uint16_t slaveId, EcSlaveConfig& outConfig) {
if (!xmlDoc) return false;
XmlNode rootNode = xmlDoc->getRoot();
if (rootNode.getNodeName() != "EtherCATInfo") {
printf("Root node is not EtherCATInfo\n");
return false;
}
if (!rootNode.hasChild("Descriptions")) {
printf("No Descriptions node found\n");
return false;
}
XmlNode descNode = rootNode.getChild("Descriptions");
outConfig = EcSlaveConfig();
outConfig.slaveId = slaveId;
if (descNode.hasChild("Devices")) {
XmlNode devicesNode = descNode.getChild("Devices");
if (devicesNode.hasChild("Device")) {
XmlNode deviceNode = devicesNode.getChild("Device");
if (deviceNode.hasChild("Type")) {
XmlNode typeNode = deviceNode.getChild("Type");
std::string productCode = typeNode.getAttrDefault("ProductCode", "");
if (!productCode.empty()) {
outConfig.productCode = (uint32_t)parseHexValue(productCode);
}
std::string revision = typeNode.getAttrDefault("RevisionNo", "");
if (!revision.empty()) {
outConfig.revisionNo = (uint32_t)atoi(revision.c_str());
}
}
if (deviceNode.hasChild("Name")) {
XmlNode nameNode = deviceNode.getChild("Name");
outConfig.name = nameNode.getNodeValue();
}
}
}
if (!descNode.hasChild("Modules")) {
printf("Warning: No Modules node found under Descriptions\n");
return !outConfig.modules.empty();
}
XmlNode modulesNode = descNode.getChild("Modules");
printf("Found Modules node under Descriptions, parsing PDO configurations...\n");
std::vector<XmlNode> moduleNodes = modulesNode.getChildren("Module");
if (moduleNodes.empty()) {
printf("Warning: No Module element found under Modules\n");
return !outConfig.modules.empty();
}
for (const auto& moduleNode : moduleNodes) {
parseModule(moduleNode, outConfig);
}
printf("Total %zu modules parsed\n", moduleNodes.size());
return true;
}
};
// ==================== EcXmlParser 公共接口 ====================
EcXmlParser::EcXmlParser() : pImpl(std::make_unique<Impl>()) {}
EcXmlParser::~EcXmlParser() = default;
bool EcXmlParser::loadXmlFile(const std::string& xmlPath) {
return pImpl->loadXmlFile(xmlPath);
}
bool EcXmlParser::parseSlaveConfig(uint16_t slaveId, EcSlaveConfig& outConfig) {
if (!pImpl->parseSlaveConfigById(slaveId, outConfig)) {
return false;
}
pImpl->slaveConfigs[slaveId] = outConfig;
return true;
}
EcSlaveConfig* EcXmlParser::getSlaveConfig(uint16_t slaveId) {
auto it = pImpl->slaveConfigs.find(slaveId);
return (it != pImpl->slaveConfigs.end()) ? &it->second : nullptr;
}
const EcSlaveConfig* EcXmlParser::getSlaveConfig(uint16_t slaveId) const {
auto it = pImpl->slaveConfigs.find(slaveId);
return (it != pImpl->slaveConfigs.end()) ? &it->second : nullptr;
}
std::vector<uint16_t> EcXmlParser::getAvailableRxPdos(uint16_t slaveId) const {
auto it = pImpl->slaveConfigs.find(slaveId);
return (it != pImpl->slaveConfigs.end()) ? it->second.availableRxPdos : std::vector<uint16_t>();
}
std::vector<uint16_t> EcXmlParser::getAvailableTxPdos(uint16_t slaveId) const {
auto it = pImpl->slaveConfigs.find(slaveId);
return (it != pImpl->slaveConfigs.end()) ? it->second.availableTxPdos : std::vector<uint16_t>();
}
const EcPdo* EcXmlParser::findPdo(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const {
auto it = pImpl->slaveConfigs.find(slaveId);
if (it == pImpl->slaveConfigs.end()) return nullptr;
for (const auto& module : it->second.modules) {
const auto& pdos = isRxPdo ? module.rxPdos : module.txPdos;
for (const auto& pdo : pdos) {
if (pdo.index == pdoIndex) return &pdo;
}
}
return nullptr;
}
const EcModule* EcXmlParser::findModuleByIndex(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const {
auto it = pImpl->slaveConfigs.find(slaveId);
if (it == pImpl->slaveConfigs.end()) return nullptr;
for (const auto& module : it->second.modules) {
const auto& pdos = isRxPdo ? module.rxPdos : module.txPdos;
for (const auto& pdo : pdos) {
if (pdo.index == pdoIndex) return &module;
}
}
return nullptr;
}
const EcModule* EcXmlParser::findModuleByIdent(uint16_t slaveId, uint32_t moduleIdent) const {
auto it = pImpl->slaveConfigs.find(slaveId);
if (it == pImpl->slaveConfigs.end()) return nullptr;
for (const auto& module : it->second.modules) {
if (module.moduleIdent == moduleIdent) return &module;
}
return nullptr;
}
void EcXmlParser::printSlaveInfo(uint16_t slaveId) const {
auto it = pImpl->slaveConfigs.find(slaveId);
if (it == pImpl->slaveConfigs.end()) {
printf("Slave %d not configured\n", slaveId);
return;
}
const EcSlaveConfig& config = it->second;
printf("\n========== Slave %d Configuration ==========\n", slaveId);
printf("Name: %s\n", config.name.c_str());
printf("Product Code: 0x%08X\n", config.productCode);
printf("Revision: %u\n", config.revisionNo);
for (const auto& module : config.modules) {
printf("\n[Module 0x%08X] %s\n", module.moduleIdent, module.name.c_str());
for (const auto& pdo : module.rxPdos) {
printf(" RxPDO 0x%04X - %s (%d entries)\n", pdo.index, pdo.name.c_str(), pdo.mapCount);
for (const auto& entry : pdo.entries) {
printf(" -> 0x%04X:%02X (%d bits) - %s\n",
entry.index, entry.subindex, entry.bitlen, entry.name.c_str());
}
}
for (const auto& pdo : module.txPdos) {
printf(" TxPDO 0x%04X - %s (%d entries)\n", pdo.index, pdo.name.c_str(), pdo.mapCount);
for (const auto& entry : pdo.entries) {
printf(" <- 0x%04X:%02X (%d bits) - %s\n",
entry.index, entry.subindex, entry.bitlen, entry.name.c_str());
}
}
}
printf("==========================================\n");
}
void EcXmlParser::clear() {
pImpl->slaveConfigs.clear();
}

41
cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h
View File

@ -1,41 +0,0 @@
// ec_xml_parser.h
#ifndef EC_XML_PARSER_H
#define EC_XML_PARSER_H
#include "ec_types.h"
#include <memory>
#include <vector>
#include <cstdint>
// XML PDO配置解析器类（仅负责解析XML）
class EcXmlParser {
public:
EcXmlParser();
~EcXmlParser();
bool loadXmlFile(const std::string& xmlPath);
bool parseSlaveConfig(uint16_t slaveId, EcSlaveConfig& outConfig);
// 获取已解析的配置
EcSlaveConfig* getSlaveConfig(uint16_t slaveId);
const EcSlaveConfig* getSlaveConfig(uint16_t slaveId) const;
// 查询接口
std::vector<uint16_t> getAvailableRxPdos(uint16_t slaveId) const;
std::vector<uint16_t> getAvailableTxPdos(uint16_t slaveId) const;
const EcPdo* findPdo(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const;
const EcModule* findModuleByIndex(uint16_t slaveId, uint16_t pdoIndex, bool isRxPdo) const;
const EcModule* findModuleByIdent(uint16_t slaveId, uint32_t moduleIdent) const;
// 打印信息
void printSlaveInfo(uint16_t slaveId) const;
// 清空所有配置
void clear();
private:
class Impl;
std::unique_ptr<Impl> pImpl;
};
#endif // EC_XML_PARSER_H

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

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

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

@ -1,46 +0,0 @@
//
// Created by cmvr on 2026/3/30.
//
#pragma once
#include "../abstract_motor.h"
#include "motor/motor_protocol_interface.h"
#include "motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h"
namespace cmvr {
namespace device {
class EYouMotor : public AbstractMotor {
public:
EYouMotor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){}
EYouMotor(const XmlNode &config):AbstractMotor(config) {
info_.id = config.getAttrDefault("slave", -1);
info_.joint_name = config.getAttrString("jointName");
info_.limitQ = config.getAttrDefault("limitQ", 3.14f);
info_.limitQd = config.getAttrDefault("limitQd", 0.5f);
node_id_ = info_.id;
}
EYouMotor(const cmvr::config::EthercatMotorConfig& config) : AbstractMotor() {
info_.id = config.slave();
info_.joint_name = config.jointname();
info_.limitQ = config.limitqlb();
info_.limitQd = config.limitqd();
node_id_ = info_.id;
}
/**
* 电机初始化在具体的类中完成
*/
void init() override {
if (!protocol_) {
throw std::runtime_error("Protocol not set for motor");
}
if (protocol_->comm_proto == MotorProtocolInterface::CommProto::ETHERCAT ) {
auto ethercat_protocol = std::dynamic_pointer_cast<EYouEthercatProtocol>(protocol_);
ethercat_protocol->initNode(node_id_);
ethercat_protocol->setQdd(node_id_,1);
}
}
};
}
}

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

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

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

@ -1,12 +1,11 @@
//
// Created by lgv on 2025/8/1.
//
#pragma once
#include "../abstract_motor.h"
#include "motor/motor_protocol_interface.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "cmvr/config/robot_config/robot_config.pb.h"
/**
* class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成
*/
@ -23,15 +22,6 @@ namespace cmvr {
node_id_ = info_.id;
}
Ti5Motor(const cmvr::config::MotorConfig& config) : AbstractMotor() {
info_.id = config.id();
info_.joint_name = config.jointname();
info_.limitQ = config.limitqlb();
info_.limitQd = config.limitqd();
node_id_ = info_.id;
}
/**
* 电机初始化在具体的类中完成
*/

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

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

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

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

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

@ -49,87 +49,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
}
}
template<int DOF>
HumanoidRobot<DOF>::HumanoidRobot(const cmvr::config::HumanRobotConfig& config) {
try {
id_ = config.id();
urdf_path_ = config.urdf();
dof_ = DOF;
if (!pathExists(config.urdf())) {
throw runtime_error("urdf file does not exist");
}
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
config.urdf(), config.baselink());
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
// 解析jointNames字符串，按逗号分隔
joint_names_ = splitString(config.jointnames(), ",");
// 解析linkNames字符串，按逗号分隔
link_names_ = splitString(config.linknames(), ",");
if (joint_names_.size() != dof_) {
throw runtime_error("joint names size mismatched with dof");
}
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
upd_freq_ = config.updfreq();
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
upd_freq_ = 1000;
upd_timer_ = make_shared<FDTimer>();
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
[this] { update_state_(); });
rsm_.store(ROBOT_READY);
} catch (exception &e) {
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
throw runtime_error(e.what());
}
}
template<int DOF>
HumanoidRobot<DOF>::HumanoidRobot(const cmvr::config::EthercatRobotConfig& config) {
try {
id_ = config.id();
urdf_path_ = config.urdf();
dof_ = DOF;
if (!pathExists(config.urdf())) {
throw runtime_error("urdf file does not exist");
}
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
config.urdf(), config.baselink());
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
// 解析jointNames字符串，按逗号分隔
joint_names_ = splitString(config.jointnames(), ",");
// 解析linkNames字符串，按逗号分隔
link_names_ = splitString(config.linknames(), ",");
if (joint_names_.size() != dof_) {
throw runtime_error("joint names size mismatched with dof");
}
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
upd_freq_ = config.updfreq();
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(config.buffersize());
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(config.buffersize());
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(config.buffersize());
upd_freq_ = 1000;
upd_timer_ = make_shared<FDTimer>();
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
[this] { update_state_(); });
rsm_.store(ROBOT_READY);
} catch (exception &e) {
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
throw runtime_error(e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::init() {
motor_manager_ = MotorsInfo::getInstance()->getMotorManager();
@ -140,7 +59,7 @@ void HumanoidRobot<DOF>::init() {
std::vector<double>(7, 1.0));
ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S",
"R_WRIST_R_S",
"R_FINGER_TIP_FIXED");

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

@ -990,21 +990,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps.";
}
TEST(HumanoidRobotTest, EYouTest) {
auto& dmgr = DeviceManager::getInstance();
auto robot_abs = dmgr.getDevice<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));
}

1
cmvr-es/main.cpp
View File

@ -9,7 +9,6 @@
#include <glog/logging.h>
#include "service/grpc/include/server_runner.h"
#include "utils/base/include/logger.h"
using namespace cmvr::service;
int main(int argc, char* argv[]) {
std::string config_path;

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

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

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

@ -8,21 +8,17 @@
#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_; // 保护共享资源的互斥锁
@ -31,4 +27,6 @@ namespace cmvr::monitor {
};
}
#endif //FOLDER_MONITOR_H

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

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

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

@ -5,8 +5,8 @@
#ifndef MONITOR_FACTORY_H
#define MONITOR_FACTORY_H
#include "monitor/abstract_monitor.h"
#include "cmvr/config/monitor_config/monitor_config.pb.h"
#include "../../monitor/abstract_monitor.h"
namespace cmvr::monitor
{
@ -14,17 +14,14 @@ namespace cmvr::monitor
{
public:
MonitorFactory() = default;
// 原有的XML方式
template <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

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

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

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

@ -3,12 +3,12 @@
//
#include "../include/monitor_factory.h"
#include "../../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,30 +17,8 @@ std::shared_ptr<MonitorType> MonitorFactory::create(const XmlNode& cfg) {
return std::make_shared<DiskMonitor>(cfg);
}
else {
LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type";
throw runtime_error("[MonitorFactory]: Unsupported monitor type");
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[DeviceFactory]: Unsupported device type");
}
}
// Proto方式的模板特化
template std::shared_ptr<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);
}

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

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

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

@ -9,7 +9,6 @@ add_library(service
grpc/src/grpc_humanoid_robot_service.cpp
grpc/src/grpc_hlc_service.cpp
grpc/src/server_runner.cpp
grpc/src/grpc_agv_service.cpp
)
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

278
cmvr-es/service/grpc/include/grpc_agv_service.h
View File

@ -1,278 +0,0 @@
//
// Created by xtkuang on 2025/6/1.
//
/**
* @file grpc_agv_service.h
* @brief gRPC AGV 服务实现类的头文件。
* 继承自自动生成的 api::AgvService::Service，提供所有 AGV 相关 RPC 接口的具体实现。
* 通过 DeviceManager 获取对应的 AGV 设备实例（如 AgvSrc1100），将请求转发至设备层。
* @author xtkuang
* @date 2025-06-01
*/
#ifndef GRPC_AGV_SERVICE_H
#define GRPC_AGV_SERVICE_H
#include "cmvr/api/agv_service.grpc.pb.h"
#include "utils/base/include/grpc_utils.h"
#include "device_manager/include/device_manager.h"
namespace cmvr::service {
/**
* @brief gRPC AGV 服务实现类。
* 负责将 gRPC 请求转换为对具体 AGV 设备的调用，并统一处理异常、日志和时间戳。
*/
class gRPCAGVServiceImpl final : public api::AgvService::Service {
public:
/**
* @brief 构造函数，获取 DeviceManager 单例引用。
*/
gRPCAGVServiceImpl();
/**
* @brief 默认析构函数。
*/
~gRPCAGVServiceImpl() override = default;
// ===================== 基本状态查询接口 =====================
/**
* @brief 获取 AGV 基本信息（命令码 1000）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID（header.device_id）。
* @param response 响应消息，填充 AGV 状态信息（版本、型号、IP、MAC 等）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status GetStatusInfo(grpc::ServerContext* context,
const api::GetAgvStatusInfoCommand_Request* request,
api::GetAgvStatusInfoCommand_Feedback* response) override;
/**
* @brief 查询电池状态（命令码 1007）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和可选的 simple 标志。
* @param response 响应消息，填充电池电量、温度、充放电状态等。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status GetBatteryStatus(grpc::ServerContext* context,
const api::RobotStatusBatteryCommand_Request* request,
api::RobotStatusBatteryCommand_Feedback* response) override;
/**
* @brief 查询机器人当前位置（命令码 1004）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID。
* @param response 响应消息，填充坐标、朝向角、置信度、当前站点等。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status GetRobotLocation(grpc::ServerContext* context,
const api::RobotStatusLocCommand_Request* request,
api::RobotStatusLocCommand_Feedback* response) override;
// ===================== 地图管理接口 =====================
/**
* @brief 下载指定地图（命令码 4011）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和地图名称。
* @param response 响应消息，返回地图 JSON 内容或错误码。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotConfigDownloadMap(grpc::ServerContext* context,
const api::RobotConfigDownloadMapCommand_Request* request,
api::RobotConfigDownloadMapCommand_Feedback* response) override;
/**
* @brief 查询地图状态（命令码 1300）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID。
* @param response 响应消息，返回当前地图名称、所有地图列表及文件详情。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status GetMapStatus(grpc::ServerContext* context,
const api::RobotStatusMapCommand_Request* request,
api::RobotStatusMapCommand_Feedback* response) override;
/**
* @brief 上传地图（命令码 4010）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和完整的地图 JSON 字符串。
* @param response 响应消息，返回上传结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotConfigUploadMap(grpc::ServerContext* context,
const api::RobotConfigUploadMapCommand_Request* request,
api::RobotConfigUploadMapCommand_Feedback* response) override;
// ===================== 控制权管理接口 =====================
/**
* @brief 抢占机器人控制权（命令码 4005）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和抢占者昵称。
* @param response 响应消息，返回抢占结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotConfigLock(grpc::ServerContext* context,
const api::RobotConfigLockCommand_Request* request,
api::RobotConfigLockCommand_Feedback* response) override;
/**
* @brief 查询当前控制权持有者（命令码 1060）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID。
* @param response 响应消息，返回是否被锁定、持有者 IP/端口/昵称等信息。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status GetCurrentLockStatus(grpc::ServerContext* context,
const api::RobotStatusCurrentLockCommand_Request* request,
api::RobotStatusCurrentLockCommand_Feedback* response) override;
// ===================== 运动控制接口 =====================
/**
* @brief 下发开环速度运动指令（命令码 2010）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和速度参数（vx, vy, w, steer, duration 等）。
* @param response 响应消息，返回指令下发结果（ret_code）。
* @note 此指令会强制取消当前自动导航任务，多舵轮设备仅 vx/vy/w 生效。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotMotionControl(grpc::ServerContext* context,
const api::RobotMotionControlCommand_Request* request,
api::RobotMotionControlCommand_Feedback* response) override;
/**
* @brief 切换载入地图（命令码 2022）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和目标地图名称。
* @param response 响应消息，返回切换结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotLoadMap(grpc::ServerContext* context,
const api::RobotLoadMapCommand_Request* request,
api::RobotLoadMapCommand_Feedback* response) override;
/**
* @brief 停止开环运动（命令码 2000）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID（无业务数据）。
* @param response 响应消息，返回停止结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotControlStop(grpc::ServerContext* context,
const api::RobotControlStopCommand::Request* request,
api::RobotControlStopCommand::Feedback* response) override;
// ===================== 导航任务接口 =====================
/**
* @brief 查询地图载入状态（命令码 1022）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID。
* @param response 响应消息，返回 loadmap_status（0=失败, 1=成功, 2=载入中）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status QueryLoadMapStatus(grpc::ServerContext* context,
const api::RobotQueryLoadMapStatusCommand_Request* request,
api::RobotQueryLoadMapStatusCommand_Feedback* response) override;
/**
* @brief 查询当前地图所有站点信息（命令码 1301）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID。
* @param response 响应消息，返回站点列表（ID、坐标、类型等）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status QueryStationList(grpc::ServerContext* context,
const api::QueryStationListCommand_Request* request,
api::QueryStationListCommand_Feedback* response) override;
/**
* @brief 指定路径导航（命令码 3066）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和多段导航任务列表（move_task_list）。
* @param response 响应消息，返回下发结果（ret_code=0 仅表示接收成功，不表示执行完成）。
* @attention 每个任务必须包含 task_id, source_id, id，且相邻站点间必须有直接路径。
* 任务会排队执行，适合多车调度场景。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotGoTargetList(grpc::ServerContext* context,
const api::RobotGoTargetListCommand_Request* request,
api::RobotGoTargetListCommand_Feedback* response) override;
/**
* @brief 查询当前实时导航状态（命令码 1020）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和 simple 标志。
* @param response 响应消息，返回任务状态、任务类型、目标、路径等信息。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotStatusTaskCurrent(grpc::ServerContext* context,
const api::RobotStatusTaskCurrentCommand_Request* request,
api::RobotStatusTaskCurrentCommand_Feedback* response) override;
/**
* @brief 批量查询任务状态（命令码 1110）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和 task_ids 列表（为空则查询所有未完成+最近完成）。
* @param response 响应消息，返回任务状态包（进度、距离、各任务状态等）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotStatusTaskPackage(grpc::ServerContext* context,
const api::RobotStatusTaskPackageCommand_Request* request,
api::RobotStatusTaskPackageCommand_Feedback* response) override;
/**
* @brief 单点站点自动规划导航（命令码 3051）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID 和完整的导航参数（支持自由导航、动作、PGV 等）。
* @param response 响应消息，返回下发结果（ret_code=0 仅表示接收成功）。
* @warning 严禁用于多车调度场景，仅限单车测试；新任务会取消当前任务（不排队）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotGoTarget(grpc::ServerContext* context,
const api::RobotGoTargetCommand_Request* request,
api::RobotGoTargetCommand_Feedback* response) override;
/**
* @brief 暂停当前导航任务（命令码 3001）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID（无业务数据）。
* @param response 响应消息，返回暂停结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotTaskPause(grpc::ServerContext* context,
const api::RobotTaskPauseCommand::Request* request,
api::RobotTaskPauseCommand::Feedback* response) override;
/**
* @brief 继续当前导航任务（命令码 3002）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID（无业务数据）。
* @param response 响应消息，返回继续结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotTaskResume(grpc::ServerContext* context,
const api::RobotTaskResumeCommand::Request* request,
api::RobotTaskResumeCommand::Feedback* response) override;
/**
* @brief 取消当前导航任务（命令码 3003）。
* @param context gRPC 上下文（未使用）。
* @param request 请求消息，包含设备 ID（无业务数据）。
* @param response 响应消息，返回取消结果（ret_code）。
* @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。
*/
grpc::Status RobotTaskCancel(grpc::ServerContext* context,
const api::RobotTaskCancelCommand::Request* request,
api::RobotTaskCancelCommand::Feedback* response) override;
private:
device::DeviceManager& dmgr_; ///< 设备管理器引用，用于根据 device_id 获取 AGV 设备实例。
};
} // namespace cmvr::service
#endif // GRPC_AGV_SERVICE_H

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

@ -11,7 +11,6 @@
#include <thread>
#include <grpcpp/grpcpp.h>
#include "rapidxml/xml_parser.h"
#include "cmvr/config/server_config/server_config.pb.h"
namespace cmvr::service {
@ -42,7 +41,6 @@ namespace cmvr::service {
std::thread worker_;
XmlNode cfg_;
cmvr::config::ServerConfig servercfg_;
std::unique_ptr<grpc::Server> server_;
std::string address_;

1208
cmvr-es/service/grpc/src/grpc_agv_service.cpp
View File

File diff suppressed because it is too large Load Diff

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

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

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

@ -22,8 +22,6 @@
#include "service/grpc/include/grpc_hlc_service.h"
#include "json/json.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
#include "service/grpc/include/grpc_agv_service.h"
#include "common/utils/config_helper/include/config_helper.h"
using namespace cmvr::service;
ServerRunner::ServerRunner() = default;
@ -99,7 +97,9 @@ 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, []() {
@ -109,8 +109,13 @@ void ServerRunner::threadMain() {
auto dmgr_cfg = cfg_.getChild("DeviceManager");
DeviceManager::getInstance(dmgr_cfg);
cmvr::ConfigHelper::getServerConfig(servercfg_);
std::string port = servercfg_.grpc_server().port();
if (cfg_.hasChild("MonitorManager")) {
auto mmgr_cfg = cfg_.getChild("MonitorManager");
MonitorManager::getInstance(mmgr_cfg);
}
auto grpc_cfg = cfg_.getChild("gRPCServer");
std::string port = grpc_cfg.getAttrDefault("port", "50051");
std::string local_address = "0.0.0.0:" + port;
auto camera_service = std::make_unique<gRPCCameraServiceImpl>();
@ -121,7 +126,6 @@ void ServerRunner::threadMain() {
auto biohand_service = std::make_unique<gRPCMBioHeadServiceImpl>();
auto humanoid_robot_service = std::make_unique<gRPCHumanoidRobotServiceImpl>();
auto hlc_service = std::make_unique<gRPCHlcServiceImpl>();
auto agv_service = std::make_unique<gRPCAGVServiceImpl>();
grpc::ServerBuilder builder;
builder.AddListeningPort(local_address, grpc::InsecureServerCredentials());
@ -133,7 +137,6 @@ void ServerRunner::threadMain() {
builder.RegisterService(biohand_service.get());
builder.RegisterService(humanoid_robot_service.get());
builder.RegisterService(hlc_service.get());
builder.RegisterService(agv_service.get());
auto local_server = builder.BuildAndStart();
if (!local_server) {

42
dependency/x86/third_party/soem/include/soem/ec_base.h vendored
View File

@ -1,42 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_base.c
*/
#ifndef _ec_base_
#define _ec_base_
#ifdef __cplusplus
extern "C" {
#endif
int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data);
uint16 ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data);
int ecx_BWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout);
int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout);
int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout);
int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout);
int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout);
int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout);
int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout);
#ifdef __cplusplus
}
#endif
#endif

73
dependency/x86/third_party/soem/include/soem/ec_coe.h vendored
View File

@ -1,73 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_coe.c
*/
#ifndef _ec_coe_
#define _ec_coe_
#ifdef __cplusplus
extern "C" {
#endif
/* Storage for object description list */
typedef struct
{
/** slave number */
uint16 Slave;
/** number of entries in list */
uint16 Entries;
/** array of indexes */
uint16 Index[EC_MAXODLIST];
/** array of datatypes, see EtherCAT specification */
uint16 DataType[EC_MAXODLIST];
/** array of object codes, see EtherCAT specification */
uint8 ObjectCode[EC_MAXODLIST];
/** number of subindexes for each index */
uint8 MaxSub[EC_MAXODLIST];
/** textual description of each index */
char Name[EC_MAXODLIST][EC_MAXNAME + 1];
} ec_ODlistt;
/* storage for object list entry information */
typedef struct
{
/** number of entries in list */
uint16 Entries;
/** array of value infos, see EtherCAT specification */
uint8 ValueInfo[EC_MAXOELIST];
/** array of value infos, see EtherCAT specification */
uint16 DataType[EC_MAXOELIST];
/** array of bit lengths, see EtherCAT specification */
uint16 BitLength[EC_MAXOELIST];
/** array of object access bits, see EtherCAT specification */
uint16 ObjAccess[EC_MAXOELIST];
/** textual description of each index */
char Name[EC_MAXOELIST][EC_MAXNAME + 1];
} ec_OElistt;
void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode);
int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex,
boolean CA, int *psize, void *p, int timeout);
int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex,
boolean CA, int psize, const void *p, int Timeout);
int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, const void *p);
int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber, int *psize, void *p, int timeout);
int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, uint32 *Osize, uint32 *Isize);
int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize);
int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist);
int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist);
int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist);
int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist);
#ifdef __cplusplus
}
#endif
#endif

31
dependency/x86/third_party/soem/include/soem/ec_config.h vendored
View File

@ -1,31 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_config.c
*/
#ifndef _ec_config_
#define _ec_config_
#ifdef __cplusplus
extern "C" {
#endif
#define EC_NODEOFFSET 0x1000
#define EC_TEMPNODE 0xffff
int ecx_config_init(ecx_contextt *context);
int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout);
int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout);
#ifdef __cplusplus
}
#endif
#endif

27
dependency/x86/third_party/soem/include/soem/ec_dc.h vendored
View File

@ -1,27 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_dc.c
*/
#ifndef _EC_ECATDC_H
#define _EC_ECATDC_H
#ifdef __cplusplus
extern "C" {
#endif
boolean ecx_configdc(ecx_contextt *context);
void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift);
void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift);
#ifdef __cplusplus
}
#endif
#endif /* _EC_ECATDC_H */

213
dependency/x86/third_party/soem/include/soem/ec_eoe.h vendored
View File

@ -1,213 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_eoe.c
*/
#ifndef _ec_eoe_
#define _ec_eoe_
#ifdef __cplusplus
extern "C" {
#endif
#include "soem/soem.h"
/* use maximum size for EOE mailbox data - mbxheader and 2x frameinfo */
#define EC_MAXEOEDATA (EC_MAXMBX - (sizeof(ec_mbxheadert) + \
sizeof(uint16_t) + \
sizeof(uint16_t)))
/** DNS length according to ETG 1000.6 */
#define EOE_DNS_NAME_LENGTH 32
/** Ethernet address length not including VLAN */
#define EOE_ETHADDR_LENGTH 6
/** IPv4 address length */
#define EOE_IP4_LENGTH sizeof(uint32_t)
#define EOE_MAKEU32(a, b, c, d) (((uint32_t)((a) & 0xff) << 24) | \
((uint32_t)((b) & 0xff) << 16) | \
((uint32_t)((c) & 0xff) << 8) | \
(uint32_t)((d) & 0xff))
#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN)
#define EOE_HTONS(x) ((((x) & 0x00ffUL) << 8) | (((x) & 0xff00UL) >> 8))
#define EOE_NTOHS(x) EOE_HTONS(x)
#define EOE_HTONL(x) ((((x) & 0x000000ffUL) << 24) | \
(((x) & 0x0000ff00UL) << 8) | \
(((x) & 0x00ff0000UL) >> 8) | \
(((x) & 0xff000000UL) >> 24))
#define EOE_NTOHL(x) EOE_HTONL(x)
#else
#define EOE_HTONS(x) (x)
#define EOE_NTOHS(x) (x)
#define EOE_HTONL(x) (x)
#define EOE_NTOHL(x) (x)
#endif /* !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) */
/** Get one byte from the 4-byte address */
#define eoe_ip4_addr1(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[0])
#define eoe_ip4_addr2(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[1])
#define eoe_ip4_addr3(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[2])
#define eoe_ip4_addr4(ipaddr) (((const uint8_t *)(&(ipaddr)->addr))[3])
/** Set an IP address given by the four byte-parts */
#define EOE_IP4_ADDR_TO_U32(ipaddr, a, b, c, d) \
(ipaddr)->addr = EOE_HTONL(EOE_MAKEU32(a, b, c, d))
/** Header frame info 1 */
#define EOE_HDR_FRAME_TYPE_OFFSET 0
#define EOE_HDR_FRAME_TYPE (0xF << 0)
#define EOE_HDR_FRAME_TYPE_SET(x) (((x) & 0xF) << 0)
#define EOE_HDR_FRAME_TYPE_GET(x) (((x) >> 0) & 0xF)
#define EOE_HDR_FRAME_PORT_OFFSET 4
#define EOE_HDR_FRAME_PORT (0xF << 4)
#define EOE_HDR_FRAME_PORT_SET(x) ((uint16)(((x) & 0xF) << 4))
#define EOE_HDR_FRAME_PORT_GET(x) (((x) >> 4) & 0xF)
#define EOE_HDR_LAST_FRAGMENT_OFFSET 8
#define EOE_HDR_LAST_FRAGMENT (0x1 << 8)
#define EOE_HDR_LAST_FRAGMENT_SET(x) (((x) & 0x1) << 8)
#define EOE_HDR_LAST_FRAGMENT_GET(x) (((x) >> 8) & 0x1)
#define EOE_HDR_TIME_APPEND_OFFSET 9
#define EOE_HDR_TIME_APPEND (0x1 << 9)
#define EOE_HDR_TIME_APPEND_SET(x) (((x) & 0x1) << 9)
#define EOE_HDR_TIME_APPEND_GET(x) (((x) >> 9) & 0x1)
#define EOE_HDR_TIME_REQUEST_OFFSET 10
#define EOE_HDR_TIME_REQUEST (0x1 << 10)
#define EOE_HDR_TIME_REQUEST_SET(x) (((x) & 0x1) << 10)
#define EOE_HDR_TIME_REQUEST_GET(x) (((x) >> 10) & 0x1)
/** Header frame info 2 */
#define EOE_HDR_FRAG_NO_OFFSET 0
#define EOE_HDR_FRAG_NO (0x3F << 0)
#define EOE_HDR_FRAG_NO_SET(x) ((uint16)(((x) & 0x3F) << 0))
#define EOE_HDR_FRAG_NO_GET(x) (((x) >> 0) & 0x3F)
#define EOE_HDR_FRAME_OFFSET_OFFSET 6
#define EOE_HDR_FRAME_OFFSET (0x3F << 6)
#define EOE_HDR_FRAME_OFFSET_SET(x) ((uint16)(((x) & 0x3F) << 6))
#define EOE_HDR_FRAME_OFFSET_GET(x) (((x) >> 6) & 0x3F)
#define EOE_HDR_FRAME_NO_OFFSET 12
#define EOE_HDR_FRAME_NO (0xF << 12)
#define EOE_HDR_FRAME_NO_SET(x) ((uint16)(((x) & 0xF) << 12))
#define EOE_HDR_FRAME_NO_GET(x) (((x) >> 12) & 0xF)
/** EOE param */
#define EOE_PARAM_OFFSET 4
#define EOE_PARAM_MAC_INCLUDE (0x1 << 0)
#define EOE_PARAM_IP_INCLUDE (0x1 << 1)
#define EOE_PARAM_SUBNET_IP_INCLUDE (0x1 << 2)
#define EOE_PARAM_DEFAULT_GATEWAY_INCLUDE (0x1 << 3)
#define EOE_PARAM_DNS_IP_INCLUDE (0x1 << 4)
#define EOE_PARAM_DNS_NAME_INCLUDE (0x1 << 5)
/** EoE frame types */
#define EOE_FRAG_DATA 0
#define EOE_INIT_RESP_TIMESTAMP 1
#define EOE_INIT_REQ 2 /* Spec SET IP REQ */
#define EOE_INIT_RESP 3 /* Spec SET IP RESP */
#define EOE_SET_ADDR_FILTER_REQ 4
#define EOE_SET_ADDR_FILTER_RESP 5
#define EOE_GET_IP_PARAM_REQ 6
#define EOE_GET_IP_PARAM_RESP 7
#define EOE_GET_ADDR_FILTER_REQ 8
#define EOE_GET_ADDR_FILTER_RESP 9
/** EoE parameter result codes */
#define EOE_RESULT_SUCCESS 0x0000
#define EOE_RESULT_UNSPECIFIED_ERROR 0x0001
#define EOE_RESULT_UNSUPPORTED_FRAME_TYPE 0x0002
#define EOE_RESULT_NO_IP_SUPPORT 0x0201
#define EOE_RESULT_NO_DHCP_SUPPORT 0x0202
#define EOE_RESULT_NO_FILTER_SUPPORT 0x0401
/** EOE ip4 address in network order */
typedef struct eoe_ip4_addr
{
uint32_t addr;
} eoe_ip4_addr_t;
/** EOE ethernet address */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED eoe_ethaddr
{
uint8_t addr[EOE_ETHADDR_LENGTH];
} eoe_ethaddr_t;
OSAL_PACKED_END
/** EoE IP request structure, storage only, no need to pack */
typedef struct eoe_param
{
uint8_t mac_set : 1;
uint8_t ip_set : 1;
uint8_t subnet_set : 1;
uint8_t default_gateway_set : 1;
uint8_t dns_ip_set : 1;
uint8_t dns_name_set : 1;
eoe_ethaddr_t mac;
eoe_ip4_addr_t ip;
eoe_ip4_addr_t subnet;
eoe_ip4_addr_t default_gateway;
eoe_ip4_addr_t dns_ip;
char dns_name[EOE_DNS_NAME_LENGTH];
} eoe_param_t;
/** EOE structure.
* Used to interpret EoE mailbox packets.
*/
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
ec_mbxheadert mbxheader;
uint16_t frameinfo1;
union
{
uint16_t frameinfo2;
uint16_t result;
};
uint8 data[EC_MAXEOEDATA];
} ec_EOEt;
OSAL_PACKED_END
int ecx_EOEdefinehook(ecx_contextt *context, void *hook);
int ecx_EOEsetIp(ecx_contextt *context,
uint16 slave,
uint8 port,
eoe_param_t *ipparam,
int timeout);
int ecx_EOEgetIp(ecx_contextt *context,
uint16 slave,
uint8 port,
eoe_param_t *ipparam,
int timeout);
int ecx_EOEsend(ecx_contextt *context,
uint16 slave,
uint8 port,
int psize,
void *p,
int timeout);
int ecx_EOErecv(ecx_contextt *context,
uint16 slave,
uint8 port,
int *psize,
void *p,
int timeout);
int ecx_EOEreadfragment(
ec_mbxbuft *MbxIn,
uint8 *rxfragmentno,
uint16 *rxframesize,
uint16 *rxframeoffset,
uint16 *rxframeno,
int *psize,
void *p);
#ifdef __cplusplus
}
#endif
#endif

27
dependency/x86/third_party/soem/include/soem/ec_foe.h vendored
View File

@ -1,27 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_foe.c
*/
#ifndef _ec_foe_
#define _ec_foe_
#ifdef __cplusplus
extern "C" {
#endif
int ecx_FOEdefinehook(ecx_contextt *context, void *hook);
int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout);
int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout);
#ifdef __cplusplus
}
#endif
#endif

624
dependency/x86/third_party/soem/include/soem/ec_main.h vendored
View File

@ -1,624 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_main.c
*/
#ifndef _ec_main_
#define _ec_main_
#ifdef __cplusplus
extern "C" {
#endif
#include "soem/ec_options.h"
typedef struct ec_adapter ec_adaptert;
struct ec_adapter
{
char name[EC_MAXLEN_ADAPTERNAME];
char desc[EC_MAXLEN_ADAPTERNAME];
ec_adaptert *next;
};
/** record for FMMU */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_fmmu
{
uint32 LogStart;
uint16 LogLength;
uint8 LogStartbit;
uint8 LogEndbit;
uint16 PhysStart;
uint8 PhysStartBit;
uint8 FMMUtype;
uint8 FMMUactive;
uint8 unused1;
uint16 unused2;
} ec_fmmut;
OSAL_PACKED_END
/** record for sync manager */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_sm
{
uint16 StartAddr;
uint16 SMlength;
uint32 SMflags;
} ec_smt;
OSAL_PACKED_END
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_state_status
{
uint16 State;
uint16 Unused;
uint16 ALstatuscode;
} ec_state_status;
OSAL_PACKED_END
/** mailbox buffer array */
typedef uint8 ec_mbxbuft[EC_MAXMBX + 1];
#define EC_MBXINENABLE (uint8 *)1
typedef struct
{
int listhead, listtail, listcount;
int mbxemptylist[EC_MBXPOOLSIZE];
osal_mutext *mbxmutex;
ec_mbxbuft mbx[EC_MBXPOOLSIZE];
} ec_mbxpoolt;
#define EC_MBXQUEUESTATE_NONE 0
#define EC_MBXQUEUESTATE_REQ 1
#define EC_MBXQUEUESTATE_FAIL 2
#define EC_MBXQUEUESTATE_DONE 3
typedef struct
{
int listhead, listtail, listcount;
ec_mbxbuft *mbx[EC_MBXPOOLSIZE];
int mbxstate[EC_MBXPOOLSIZE];
int mbxremove[EC_MBXPOOLSIZE];
int mbxticket[EC_MBXPOOLSIZE];
uint16 mbxslave[EC_MBXPOOLSIZE];
osal_mutext *mbxmutex;
} ec_mbxqueuet;
#define ECT_MBXPROT_AOE 0x0001
#define ECT_MBXPROT_EOE 0x0002
#define ECT_MBXPROT_COE 0x0004
#define ECT_MBXPROT_FOE 0x0008
#define ECT_MBXPROT_SOE 0x0010
#define ECT_MBXPROT_VOE 0x0020
#define ECT_COEDET_SDO 0x01
#define ECT_COEDET_SDOINFO 0x02
#define ECT_COEDET_PDOASSIGN 0x04
#define ECT_COEDET_PDOCONFIG 0x08
#define ECT_COEDET_UPLOAD 0x10
#define ECT_COEDET_SDOCA 0x20
#define EC_SMENABLEMASK 0xfffeffff
typedef struct ecx_context ecx_contextt;
#define ECT_MBXH_NONE 0
#define ECT_MBXH_CYCLIC 1
#define ECT_MBXH_LOST 2
/** Slave state
* All slave information is put in this structure. Needed for most
* user interaction with slaves.
*/
typedef struct ec_slave
{
/** state of slave */
uint16 state;
/** AL status code */
uint16 ALstatuscode;
/** Configured address */
uint16 configadr;
/** Alias address */
uint16 aliasadr;
/** Manufacturer from EEprom */
uint32 eep_man;
/** ID from EEprom */
uint32 eep_id;
/** revision from EEprom */
uint32 eep_rev;
/** serial number from EEprom */
uint32 eep_ser;
/** Interface type */
uint16 Itype;
/** Device type */
uint16 Dtype;
/** output bits */
uint16 Obits;
/** output bytes, if Obits < 8 then Obytes = 0 */
uint32 Obytes;
/** output pointer in IOmap buffer */
uint8 *outputs;
/** output offset in IOmap buffer */
uint32 Ooffset;
/** startbit in first output byte */
uint8 Ostartbit;
/** input bits */
uint16 Ibits;
/** input bytes, if Ibits < 8 then Ibytes = 0 */
uint32 Ibytes;
/** input pointer in IOmap buffer */
uint8 *inputs;
/** input offset in IOmap buffer */
uint32 Ioffset;
/** startbit in first input byte */
uint8 Istartbit;
/** SM structure */
ec_smt SM[EC_MAXSM];
/** SM type 0=unused 1=MbxWr 2=MbxRd 3=Outputs 4=Inputs */
uint8 SMtype[EC_MAXSM];
/** FMMU structure */
ec_fmmut FMMU[EC_MAXFMMU];
/** FMMU0 function 0=unused 1=outputs 2=inputs 3=SM status*/
uint8 FMMU0func;
/** FMMU1 function */
uint8 FMMU1func;
/** FMMU2 function */
uint8 FMMU2func;
/** FMMU3 function */
uint8 FMMU3func;
/** length of write mailbox in bytes, if no mailbox then 0 */
uint16 mbx_l;
/** mailbox write offset */
uint16 mbx_wo;
/** length of read mailbox in bytes */
uint16 mbx_rl;
/** mailbox read offset */
uint16 mbx_ro;
/** mailbox supported protocols */
uint16 mbx_proto;
/** Counter value of mailbox link layer protocol 1..7 */
uint8 mbx_cnt;
/** has DC capability */
boolean hasdc;
/** Physical type; Ebus, EtherNet combinations */
uint8 ptype;
/** topology: 1 to 3 links */
uint8 topology;
/** active ports bitmap : ....3210 , set if respective port is active **/
uint8 activeports;
/** consumed ports bitmap : ....3210, used for internal delay measurement **/
uint8 consumedports;
/** slave number for parent, 0=master */
uint16 parent;
/** port number on parent this slave is connected to **/
uint8 parentport;
/** port number on this slave the parent is connected to **/
uint8 entryport;
/** DC receivetimes on port A */
int32 DCrtA;
/** DC receivetimes on port B */
int32 DCrtB;
/** DC receivetimes on port C */
int32 DCrtC;
/** DC receivetimes on port D */
int32 DCrtD;
/** propagation delay */
int32 pdelay;
/** next DC slave */
uint16 DCnext;
/** previous DC slave */
uint16 DCprevious;
/** DC cycle time in ns */
int32 DCcycle;
/** DC shift from clock modulus boundary */
int32 DCshift;
/** DC sync activation, 0=off, 1=on */
uint8 DCactive;
/** link to SII config */
uint16 SIIindex;
/** 1 = 8 bytes per read, 0 = 4 bytes per read */
uint8 eep_8byte;
/** 0 = eeprom to master , 1 = eeprom to PDI */
uint8 eep_pdi;
/** CoE details */
uint8 CoEdetails;
/** FoE details */
uint8 FoEdetails;
/** EoE details */
uint8 EoEdetails;
/** SoE details */
uint8 SoEdetails;
/** E-bus current */
int16 Ebuscurrent;
/** if >0 block use of LRW in processdata */
uint8 blockLRW;
/** group */
uint8 group;
/** first unused FMMU */
uint8 FMMUunused;
/** Boolean for tracking whether the slave is (not) responding, not used/set by the SOEM library */
boolean islost;
/** registered configuration function PO->SO */
int (*PO2SOconfig)(ecx_contextt *context, uint16 slave);
/** mailbox handler state, 0 = no handler, 1 = cyclic task mbx handler, 2 = slave lost */
int mbxhandlerstate;
/** mailbox handler robust mailbox protocol state */
int mbxrmpstate;
/** mailbox handler RMP extended mbx in state */
uint16 mbxinstateex;
/** pointer to CoE mailbox in buffer */
uint8 *coembxin;
/** CoE mailbox in flag, true = mailbox full */
boolean coembxinfull;
/** CoE mailbox in overrun counter */
int coembxoverrun;
/** pointer to SoE mailbox in buffer */
uint8 *soembxin;
/** SoE mailbox in flag, true = mailbox full */
boolean soembxinfull;
/** SoE mailbox in overrun counter */
int soembxoverrun;
/** pointer to FoE mailbox in buffer */
uint8 *foembxin;
/** FoE mailbox in flag, true = mailbox full */
boolean foembxinfull;
/** FoE mailbox in overrun counter */
int foembxoverrun;
/** pointer to EoE mailbox in buffer */
uint8 *eoembxin;
/** EoE mailbox in flag, true = mailbox full */
boolean eoembxinfull;
/** EoE mailbox in overrun counter */
int eoembxoverrun;
/** pointer to VoE mailbox in buffer */
uint8 *voembxin;
/** VoE mailbox in flag, true = mailbox full */
boolean voembxinfull;
/** VoE mailbox in overrun counter */
int voembxoverrun;
/** pointer to AoE mailbox in buffer */
uint8 *aoembxin;
/** AoE mailbox in flag, true = mailbox full */
boolean aoembxinfull;
/** AoE mailbox in overrun counter */
int aoembxoverrun;
/** pointer to out mailbox status register buffer */
uint8 *mbxstatus;
/** readable name */
char name[EC_MAXNAME + 1];
} ec_slavet;
/** for list of ethercat slave groups */
typedef struct ec_group
{
/** logical start address for this group */
uint32 logstartaddr;
/** output bytes, if Obits < 8 then Obytes = 0 */
uint32 Obytes;
/** output pointer in IOmap buffer */
uint8 *outputs;
/** input bytes, if Ibits < 8 then Ibytes = 0 */
uint32 Ibytes;
/** input pointer in IOmap buffer */
uint8 *inputs;
/** has DC capability */
boolean hasdc;
/** next DC slave */
uint16 DCnext;
/** E-bus current */
int16 Ebuscurrent;
/** if >0 block use of LRW in processdata */
uint8 blockLRW;
/** IO segments used */
uint16 nsegments;
/** 1st input segment */
uint16 Isegment;
/** Offset in input segment */
uint16 Ioffset;
/** Expected workcounter outputs */
uint16 outputsWKC;
/** Expected workcounter inputs */
uint16 inputsWKC;
/** check slave states */
boolean docheckstate;
/** IO segmentation list. Datagrams must not break SM in two. */
uint32 IOsegment[EC_MAXIOSEGMENTS];
/** pointer to out mailbox status register buffer */
uint8 *mbxstatus;
/** mailbox status register buffer length */
int32 mbxstatuslength;
/** mailbox status lookup table */
uint16 mbxstatuslookup[EC_MAXSLAVE];
/** mailbox last handled in mxbhandler */
uint16 lastmbxpos;
/** mailbox transmit queue struct */
ec_mbxqueuet mbxtxqueue;
} ec_groupt;
#define ECT_ESMTRANS_IP 0x0001
#define ECT_ESMTRANS_PS 0x0002
#define ECT_ESMTRANS_PI 0x0004
#define ECT_ESMTRANS_SP 0x0008
#define ECT_ESMTRANS_SO 0x0010
#define ECT_ESMTRANS_SI 0x0020
#define ECT_ESMTRANS_OS 0x0040
#define ECT_ESMTRANS_OP 0x0080
#define ECT_ESMTRANS_OI 0x0100
#define ECT_ESMTRANS_IB 0x0200
#define ECT_ESMTRANS_BI 0x0400
#define ECT_ESMTRANS_II 0x0800
#define ECT_ESMTRANS_PP 0x1000
#define ECT_ESMTRANS_SS 0x2000
/** ENI CoE command structure */
typedef struct ec_enicoecmd
{
/** transition(s) during which command should be sent */
uint16 Transition;
/** complete access flag */
boolean CA;
/** ccs (1 = read, 2 = write) */
uint8 Ccs;
/** object index */
uint16 Index;
/** object subindex */
uint8 SubIdx;
/** timeout in us */
int Timeout;
/** size in bytes of parameter buffer */
int DataSize;
/** pointer to parameter buffer */
void *Data;
} ec_enicoecmdt;
/** ENI slave structure */
typedef struct ec_enislave
{
uint16 Slave;
uint32 VendorId;
uint32 ProductCode;
uint32 RevisionNo;
ec_enicoecmdt *CoECmds;
int CoECmdCount;
} ec_enislavet;
/** ENI structure */
typedef struct ec_eni
{
ec_enislavet *slave;
int slavecount;
} ec_enit;
/** SII FMMU structure */
typedef struct ec_eepromFMMU
{
uint16 Startpos;
uint8 nFMMU;
uint8 FMMU0;
uint8 FMMU1;
uint8 FMMU2;
uint8 FMMU3;
} ec_eepromFMMUt;
/** SII SM structure */
typedef struct ec_eepromSM
{
uint16 Startpos;
uint8 nSM;
uint16 PhStart;
uint16 Plength;
uint8 Creg;
uint8 Sreg; /* don't care */
uint8 Activate;
uint8 PDIctrl; /* don't care */
} ec_eepromSMt;
/** record to store rxPDO and txPDO table from eeprom */
typedef struct ec_eepromPDO
{
uint16 Startpos;
uint16 Length;
uint16 nPDO;
uint16 Index[EC_MAXEEPDO];
uint16 SyncM[EC_MAXEEPDO];
uint16 BitSize[EC_MAXEEPDO];
uint16 SMbitsize[EC_MAXSM];
} ec_eepromPDOt;
/** standard ethercat mailbox header */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_mbxheader
{
uint16 length;
uint16 address;
uint8 priority;
uint8 mbxtype;
} ec_mbxheadert;
OSAL_PACKED_END
/** ALstatus and ALstatus code */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_alstatus
{
uint16 alstatus;
uint16 unused;
uint16 alstatuscode;
} ec_alstatust;
OSAL_PACKED_END
/** stack structure to store segmented LRD/LWR/LRW constructs */
typedef struct ec_idxstack
{
uint8 pushed;
uint8 pulled;
uint8 idx[EC_MAXBUF];
void *data[EC_MAXBUF];
uint16 length[EC_MAXBUF];
uint16 dcoffset[EC_MAXBUF];
uint8 type[EC_MAXBUF];
} ec_idxstackT;
/** ringbuf for error storage */
typedef struct ec_ering
{
int16 head;
int16 tail;
ec_errort Error[EC_MAXELIST + 1];
} ec_eringt;
/** SyncManager Communication Type structure for CA */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_SMcommtype
{
uint8 n;
uint8 nu1;
uint8 SMtype[EC_MAXSM];
} ec_SMcommtypet;
OSAL_PACKED_END
/** SDO assign structure for CA */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_PDOassign
{
uint8 n;
uint8 nu1;
uint16 index[256];
} ec_PDOassignt;
OSAL_PACKED_END
/** SDO description structure for CA */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED ec_PDOdesc
{
uint8 n;
uint8 nu1;
uint32 PDO[256];
} ec_PDOdesct;
OSAL_PACKED_END
/** Context structure, referenced by all ecx functions*/
struct ecx_context
{
/** @publicsection */
/* Network state */
/** port, may include red_port */
ecx_portt port;
/** list of detected slaves */
ec_slavet slavelist[EC_MAXSLAVE];
/** number of slaves found in configuration */
int slavecount;
/** list of groups */
ec_groupt grouplist[EC_MAXGROUP];
/** ecaterror state */
boolean ecaterror;
/** last DC time from slaves */
int64 DCtime;
/** @privatesection */
/* Internal state */
/** internal, eeprom cache buffer */
uint8 esibuf[EC_MAXEEPBUF];
/** internal, eeprom cache map */
uint32 esimap[EC_MAXEEPBITMAP];
/** internal, current slave for eeprom cache */
uint16 esislave;
/** internal, error list */
ec_eringt elist;
/** internal, processdata stack buffer info */
ec_idxstackT idxstack;
/** internal, SM buffer */
ec_SMcommtypet SMcommtype[EC_MAX_MAPT];
/** internal, PDO assign list */
ec_PDOassignt PDOassign[EC_MAX_MAPT];
/** internal, PDO description list */
ec_PDOdesct PDOdesc[EC_MAX_MAPT];
/** internal, SM list from eeprom */
ec_eepromSMt eepSM;
/** internal, FMMU list from eeprom */
ec_eepromFMMUt eepFMMU;
/** internal, mailbox pool */
ec_mbxpoolt mbxpool;
/** @publicsection */
/* Configurable settings */
/** network information hook */
ec_enit *ENI;
/** registered FoE hook */
int (*FOEhook)(uint16 slave, int packetnumber, int datasize);
/** registered EoE hook */
int (*EOEhook)(ecx_contextt *context, uint16 slave, void *eoembx);
/** flag to control legacy automatic state change or manual state change */
int manualstatechange;
/** opaque pointer to application userdata, never used by SOEM. */
void *userdata;
/** In overlapped mode, inputs will replace outputs in the incoming
* frame. Use this mode for TI ESC:s. Processdata is always aligned
* on a byte boundary. */
boolean overlappedMode;
/** Do not map each slave on a byte boundary. May result in smaller
* frame sizes. Has no effect in overlapped mode. */
boolean packedMode;
};
ec_adaptert *ec_find_adapters(void);
void ec_free_adapters(ec_adaptert *adapter);
uint8 ec_nextmbxcnt(uint8 cnt);
void ec_clearmbx(ec_mbxbuft *Mbx);
void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec);
boolean ecx_poperror(ecx_contextt *context, ec_errort *Ec);
boolean ecx_iserror(ecx_contextt *context);
void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode);
int ecx_init(ecx_contextt *context, const char *ifname);
int ecx_init_redundant(ecx_contextt *context, ecx_redportt *redport, const char *ifname, char *if2name);
void ecx_close(ecx_contextt *context);
uint8 ecx_siigetbyte(ecx_contextt *context, uint16 slave, uint16 address);
int16 ecx_siifind(ecx_contextt *context, uint16 slave, uint16 cat);
void ecx_siistring(ecx_contextt *context, char *str, uint16 slave, uint16 Sn);
uint16 ecx_siiFMMU(ecx_contextt *context, uint16 slave, ec_eepromFMMUt *FMMU);
uint16 ecx_siiSM(ecx_contextt *context, uint16 slave, ec_eepromSMt *SM);
uint16 ecx_siiSMnext(ecx_contextt *context, uint16 slave, ec_eepromSMt *SM, uint16 n);
uint32 ecx_siiPDO(ecx_contextt *context, uint16 slave, ec_eepromPDOt *PDO, uint8 t);
int ecx_readstate(ecx_contextt *context);
int ecx_writestate(ecx_contextt *context, uint16 slave);
uint16 ecx_statecheck(ecx_contextt *context, uint16 slave, uint16 reqstate, int timeout);
int ecx_mbxhandler(ecx_contextt *context, uint8 group, int limit);
int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout);
int ecx_mbxsend(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout);
int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft **mbx, int timeout);
int ecx_mbxENIinitcmds(ecx_contextt *context, uint16 slave, uint16_t transition);
void ecx_esidump(ecx_contextt *context, uint16 slave, uint8 *esibuf);
uint32 ecx_readeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, int timeout);
int ecx_writeeeprom(ecx_contextt *context, uint16 slave, uint16 eeproma, uint16 data, int timeout);
int ecx_eeprom2master(ecx_contextt *context, uint16 slave);
int ecx_eeprom2pdi(ecx_contextt *context, uint16 slave);
uint64 ecx_readeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, int timeout);
int ecx_writeeepromAP(ecx_contextt *context, uint16 aiadr, uint16 eeproma, uint16 data, int timeout);
uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, int timeout);
int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout);
void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma);
uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout);
int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout);
int ecx_send_processdata(ecx_contextt *context);
int ecx_receive_processdata(ecx_contextt *context, int timeout);
int ecx_send_processdata_group(ecx_contextt *context, uint8 group);
ec_mbxbuft *ecx_getmbx(ecx_contextt *context);
int ecx_dropmbx(ecx_contextt *context, ec_mbxbuft *mbx);
int ecx_initmbxpool(ecx_contextt *context);
int ecx_initmbxqueue(ecx_contextt *context, uint8 group);
int ecx_slavembxcyclic(ecx_contextt *context, uint16 slave);
#ifdef __cplusplus
}
#endif
#endif

128
dependency/x86/third_party/soem/include/soem/ec_options.h vendored
View File

@ -1,128 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Build options for SOEM
*/
#ifndef _ec_options_
#define _ec_options_
#ifdef __cplusplus
extern "C" {
#endif
/* Max sizes */
/** standard frame buffer size in bytes */
#define EC_BUFSIZE (EC_MAXECATFRAME)
/** number of frame buffers per channel (tx, rx1 rx2) */
#define EC_MAXBUF (16)
/** size of EEPROM bitmap cache */
#define EC_MAXEEPBITMAP (128)
/** size of EEPROM cache buffer */
#define EC_MAXEEPBUF (EC_MAXEEPBITMAP << 5)
/** default group size in 2^x */
#define EC_LOGGROUPOFFSET (16)
/** max. entries in EtherCAT error list */
#define EC_MAXELIST (64)
/** max. length of readable name in slavelist and Object Description List */
#define EC_MAXNAME (40)
/** max. number of slaves in array */
#define EC_MAXSLAVE (200)
/** max. number of groups */
#define EC_MAXGROUP (2)
/** max. number of IO segments per group */
#define EC_MAXIOSEGMENTS (64)
/** max. mailbox size */
#define EC_MAXMBX (1486)
/** number of mailboxes in pool */
#define EC_MBXPOOLSIZE (32)
/** max. eeprom PDO entries */
#define EC_MAXEEPDO (0x200)
/** max. SM used */
#define EC_MAXSM (8)
/** max. FMMU used */
#define EC_MAXFMMU (4)
/** max. adapter name length */
#define EC_MAXLEN_ADAPTERNAME (128)
/** define maximum number of concurrent threads in mapping */
#define EC_MAX_MAPT (1)
/** max entries in Object Description list */
#define EC_MAXODLIST (1024)
/** max entries in Object Entry list */
#define EC_MAXOELIST (256)
/** max. length of readable SoE name */
#define EC_SOE_MAXNAME (60)
/** max. number of SoE mappings */
#define EC_SOE_MAXMAPPING (64)
/* Timeouts and retries */
/** timeout value in us for tx frame to return to rx */
#define EC_TIMEOUTRET (2000)
/** timeout value in us for safe data transfer, max. triple retry */
#define EC_TIMEOUTRET3 (EC_TIMEOUTRET * 3)
/** timeout value in us for return "safe" variant (f.e. wireless) */
#define EC_TIMEOUTSAFE (20000)
/** timeout value in us for EEPROM access */
#define EC_TIMEOUTEEP (20000)
/** timeout value in us for tx mailbox cycle */
#define EC_TIMEOUTTXM (20000)
/** timeout value in us for rx mailbox cycle */
#define EC_TIMEOUTRXM (700000)
/** timeout value in us for check statechange */
#define EC_TIMEOUTSTATE (2000000)
/** default number of retries if wkc <= 0 */
#define EC_DEFAULTRETRIES (3)
/* MAC addresses */
/** Primary source MAC address used for EtherCAT.
*
* This address is not the MAC address used from the NIC. EtherCAT
* does not care about MAC addressing, but it is used here to
* differentiate the route the packet traverses through the EtherCAT
* segment. This is needed to find out the packet flow in redundant
* configurations. */
#define EC_PRIMARY_MAC_ARRAY {0x0101, 0x0101, 0x0101}
/** Secondary source MAC address used for EtherCAT. */
#define EC_SECONDARY_MAC_ARRAY {0x0404, 0x0404, 0x0404}
#ifdef __cplusplus
}
#endif
#endif

30
dependency/x86/third_party/soem/include/soem/ec_print.h vendored
View File

@ -1,30 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_print.c
*/
#ifndef _ec_print_
#define _ec_print_
#ifdef __cplusplus
extern "C" {
#endif
const char *ec_sdoerror2string(uint32 sdoerrorcode);
char *ec_ALstatuscode2string(uint16 ALstatuscode);
char *ec_soeerror2string(uint16 errorcode);
char *ec_mbxerror2string(uint16 errorcode);
char *ecx_err2string(const ec_errort Ec);
char *ecx_elist2string(ecx_contextt *context);
#ifdef __cplusplus
}
#endif
#endif

121
dependency/x86/third_party/soem/include/soem/ec_soe.h vendored
View File

@ -1,121 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for ec_soe.c
*/
#ifndef _ec_soe_
#define _ec_soe_
#ifdef __cplusplus
extern "C" {
#endif
#define EC_SOE_DATASTATE_B 0x01
#define EC_SOE_NAME_B 0x02
#define EC_SOE_ATTRIBUTE_B 0x04
#define EC_SOE_UNIT_B 0x08
#define EC_SOE_MIN_B 0x10
#define EC_SOE_MAX_B 0x20
#define EC_SOE_VALUE_B 0x40
#define EC_SOE_DEFAULT_B 0x80
#define EC_IDN_MDTCONFIG 24
#define EC_IDN_ATCONFIG 16
/** SoE name structure */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** current length in bytes of list */
uint16 currentlength;
/** maximum length in bytes of list */
uint16 maxlength;
char name[EC_SOE_MAXNAME];
} ec_SoEnamet;
OSAL_PACKED_END
/** SoE list structure */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** current length in bytes of list */
uint16 currentlength;
/** maximum length in bytes of list */
uint16 maxlength;
union
{
uint8 byte[8];
uint16 word[4];
uint32 dword[2];
uint64 lword[1];
};
} ec_SoElistt;
OSAL_PACKED_END
/** SoE IDN mapping structure */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** current length in bytes of list */
uint16 currentlength;
/** maximum length in bytes of list */
uint16 maxlength;
uint16 idn[EC_SOE_MAXMAPPING];
} ec_SoEmappingt;
OSAL_PACKED_END
#define EC_SOE_LENGTH_1 0x00
#define EC_SOE_LENGTH_2 0x01
#define EC_SOE_LENGTH_4 0x02
#define EC_SOE_LENGTH_8 0x03
#define EC_SOE_TYPE_BINARY 0x00
#define EC_SOE_TYPE_UINT 0x01
#define EC_SOE_TYPE_INT 0x02
#define EC_SOE_TYPE_HEX 0x03
#define EC_SOE_TYPE_STRING 0x04
#define EC_SOE_TYPE_IDN 0x05
#define EC_SOE_TYPE_FLOAT 0x06
#define EC_SOE_TYPE_PARAMETER 0x07
/** SoE attribute structure */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** evaluation factor for display purposes */
uint32 evafactor : 16;
/** length of IDN element(s) */
uint32 length : 2;
/** IDN is list */
uint32 list : 1;
/** IDN is command */
uint32 command : 1;
/** datatype */
uint32 datatype : 3;
uint32 reserved1 : 1;
/** decimals to display if float datatype */
uint32 decimals : 4;
/** write protected in pre-op */
uint32 wppreop : 1;
/** write protected in safe-op */
uint32 wpsafeop : 1;
/** write protected in op */
uint32 wpop : 1;
uint32 reserved2 : 1;
} ec_SoEattributet;
OSAL_PACKED_END
int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout);
int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout);
int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize);
#ifdef __cplusplus
}
#endif
#endif

551
dependency/x86/third_party/soem/include/soem/ec_type.h vendored
View File

@ -1,551 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* General typedefs and defines for EtherCAT.
*
* Defines that could need optimisation for specific applications
* are the EC_TIMEOUTxxx. Assumptions for the standard settings are a
* standard linux PC or laptop and a wired connection to maximal 100 slaves.
* For use with wireless connections or lots of slaves the timeouts need
* increasing. For fast systems running Xenomai and RT-net or alike the
* timeouts need to be shorter.
*/
#ifndef _EC_TYPE_H
#define _EC_TYPE_H
#ifdef __cplusplus
extern "C" {
#endif
#include "osal.h"
#include "soem/ec_options.h"
/* Endianness */
/** Define little endian target by default if no endian is set */
#if !defined(EC_LITTLE_ENDIAN) && !defined(EC_BIG_ENDIAN)
#define EC_LITTLE_ENDIAN
#endif
/* Error codes */
/** return value no frame returned */
#define EC_NOFRAME -1
/** return value unknown frame received */
#define EC_OTHERFRAME -2
/** return value general error */
#define EC_ERROR -3
/** return value too many slaves */
#define EC_SLAVECOUNTEXCEEDED -4
/** return value request timeout */
#define EC_TIMEOUT -5
/* Constants */
/** maximum EtherCAT frame length in bytes */
#define EC_MAXECATFRAME 1518
/** size of DC datagram used in first LRW frame */
#define EC_FIRSTDCDATAGRAM 20
/** datagram type EtherCAT */
#define EC_ECATTYPE 0x1000
/** maximum EtherCAT LRW frame length in bytes */
/* MTU - Ethernet header - length - datagram header - WCK - FCS */
#define EC_MAXLRWDATA (EC_MAXECATFRAME - 14 - 2 - 10 - 2 - 4)
/** definition for frame buffers */
typedef uint8 ec_bufT[EC_BUFSIZE];
/** ethernet header definition */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** destination MAC */
uint16 da0, da1, da2;
/** source MAC */
uint16 sa0, sa1, sa2;
/** ethernet type */
uint16 etype;
} ec_etherheadert;
OSAL_PACKED_END
/** ethernet header size */
#define ETH_HEADERSIZE sizeof(ec_etherheadert)
/** EtherCAT datagram header definition */
OSAL_PACKED_BEGIN
typedef struct OSAL_PACKED
{
/** length of EtherCAT datagram */
uint16 elength;
/** EtherCAT command, see ec_cmdtype */
uint8 command;
/** index, used in SOEM for Tx to Rx recombination */
uint8 index;
/** ADP */
uint16 ADP;
/** ADO */
uint16 ADO;
/** length of data portion in datagram */
uint16 dlength;
/** interrupt, currently unused */
uint16 irpt;
} ec_comt;
OSAL_PACKED_END
/** EtherCAT header size */
#define EC_HEADERSIZE sizeof(ec_comt)
/** size of ec_comt.elength item in EtherCAT header */
#define EC_ELENGTHSIZE sizeof(uint16)
/** offset position of command in EtherCAT header */
#define EC_CMDOFFSET EC_ELENGTHSIZE
/** size of workcounter item in EtherCAT datagram */
#define EC_WKCSIZE sizeof(uint16)
/** definition of datagram follows bit in ec_comt.dlength */
#define EC_DATAGRAMFOLLOWS (1 << 15)
/** Possible error codes returned. */
typedef enum
{
/** No error */
EC_ERR_OK = 0,
/** Library already initialized. */
EC_ERR_ALREADY_INITIALIZED,
/** Library not initialized. */
EC_ERR_NOT_INITIALIZED,
/** Timeout occurred during execution of the function. */
EC_ERR_TIMEOUT,
/** No slaves were found. */
EC_ERR_NO_SLAVES,
/** Function failed. */
EC_ERR_NOK
} ec_err;
/** Possible EtherCAT slave states */
typedef enum
{
/** No valid state. */
EC_STATE_NONE = 0x00,
/** Init state*/
EC_STATE_INIT = 0x01,
/** Pre-operational. */
EC_STATE_PRE_OP = 0x02,
/** Boot state*/
EC_STATE_BOOT = 0x03,
/** Safe-operational. */
EC_STATE_SAFE_OP = 0x04,
/** Operational */
EC_STATE_OPERATIONAL = 0x08,
/** Error or ACK error */
EC_STATE_ACK = 0x10,
EC_STATE_ERROR = 0x10
} ec_state;
/** Possible buffer states */
typedef enum
{
/** Empty */
EC_BUF_EMPTY = 0x00,
/** Allocated, but not filled */
EC_BUF_ALLOC = 0x01,
/** Transmitted */
EC_BUF_TX = 0x02,
/** Received, but not consumed */
EC_BUF_RCVD = 0x03,
/** Cycle completed */
EC_BUF_COMPLETE = 0x04
} ec_bufstate;
/** Ethercat data types */
typedef enum
{
ECT_BOOLEAN = 0x0001,
ECT_INTEGER8 = 0x0002,
ECT_INTEGER16 = 0x0003,
ECT_INTEGER32 = 0x0004,
ECT_UNSIGNED8 = 0x0005,
ECT_UNSIGNED16 = 0x0006,
ECT_UNSIGNED32 = 0x0007,
ECT_REAL32 = 0x0008,
ECT_VISIBLE_STRING = 0x0009,
ECT_OCTET_STRING = 0x000A,
ECT_UNICODE_STRING = 0x000B,
ECT_TIME_OF_DAY = 0x000C,
ECT_TIME_DIFFERENCE = 0x000D,
ECT_DOMAIN = 0x000F,
ECT_INTEGER24 = 0x0010,
ECT_REAL64 = 0x0011,
ECT_INTEGER64 = 0x0015,
ECT_UNSIGNED24 = 0x0016,
ECT_UNSIGNED64 = 0x001B,
ECT_BIT1 = 0x0030,
ECT_BIT2 = 0x0031,
ECT_BIT3 = 0x0032,
ECT_BIT4 = 0x0033,
ECT_BIT5 = 0x0034,
ECT_BIT6 = 0x0035,
ECT_BIT7 = 0x0036,
ECT_BIT8 = 0x0037
} ec_datatype;
/** Ethercat command types */
typedef enum
{
/** No operation */
EC_CMD_NOP = 0x00,
/** Auto Increment Read */
EC_CMD_APRD,
/** Auto Increment Write */
EC_CMD_APWR,
/** Auto Increment Read Write */
EC_CMD_APRW,
/** Configured Address Read */
EC_CMD_FPRD,
/** Configured Address Write */
EC_CMD_FPWR,
/** Configured Address Read Write */
EC_CMD_FPRW,
/** Broadcast Read */
EC_CMD_BRD,
/** Broadcast Write */
EC_CMD_BWR,
/** Broadcast Read Write */
EC_CMD_BRW,
/** Logical Memory Read */
EC_CMD_LRD,
/** Logical Memory Write */
EC_CMD_LWR,
/** Logical Memory Read Write */
EC_CMD_LRW,
/** Auto Increment Read Multiple Write */
EC_CMD_ARMW,
/** Configured Read Multiple Write */
EC_CMD_FRMW
/** Reserved */
} ec_cmdtype;
/** Ethercat EEprom command types */
typedef enum
{
/** No operation */
EC_ECMD_NOP = 0x0000,
/** Read */
EC_ECMD_READ = 0x0100,
/** Write */
EC_ECMD_WRITE = 0x0201,
/** Reload */
EC_ECMD_RELOAD = 0x0300
} ec_ecmdtype;
/** EEprom state machine read size */
#define EC_ESTAT_R64 0x0040
/** EEprom state machine busy flag */
#define EC_ESTAT_BUSY 0x8000
/** EEprom state machine error flag mask */
#define EC_ESTAT_EMASK 0x7800
/** EEprom state machine error acknowledge */
#define EC_ESTAT_NACK 0x2000
/* Ethercat SSI (Slave Information Interface) */
/** Start address SII sections in Eeprom */
#define ECT_SII_START 0x0040
enum
{
/** SII category strings */
ECT_SII_STRING = 10,
/** SII category general */
ECT_SII_GENERAL = 30,
/** SII category FMMU */
ECT_SII_FMMU = 40,
/** SII category SM */
ECT_SII_SM = 41,
/** SII category PDO */
ECT_SII_PDO = 50
};
/** Item offsets in SII general section */
enum
{
ECT_SII_MANUF = 0x0008,
ECT_SII_ID = 0x000a,
ECT_SII_REV = 0x000c,
ECT_SII_SER = 0x000e,
ECT_SII_BOOTRXMBX = 0x0014,
ECT_SII_BOOTTXMBX = 0x0016,
ECT_SII_MBXSIZE = 0x0019,
ECT_SII_TXMBXADR = 0x001a,
ECT_SII_RXMBXADR = 0x0018,
ECT_SII_MBXPROTO = 0x001c
};
/** Mailbox types definitions */
enum
{
/** Error mailbox type */
ECT_MBXT_ERR = 0x00,
/** ADS over EtherCAT mailbox type */
ECT_MBXT_AOE,
/** Ethernet over EtherCAT mailbox type */
ECT_MBXT_EOE,
/** CANopen over EtherCAT mailbox type */
ECT_MBXT_COE,
/** File over EtherCAT mailbox type */
ECT_MBXT_FOE,
/** Servo over EtherCAT mailbox type */
ECT_MBXT_SOE,
/** Vendor over EtherCAT mailbox type */
ECT_MBXT_VOE = 0x0f
};
/** CoE mailbox types */
enum
{
ECT_COES_EMERGENCY = 0x01,
ECT_COES_SDOREQ,
ECT_COES_SDORES,
ECT_COES_TXPDO,
ECT_COES_RXPDO,
ECT_COES_TXPDO_RR,
ECT_COES_RXPDO_RR,
ECT_COES_SDOINFO
};
/** CoE SDO commands */
enum
{
ECT_SDO_DOWN_INIT = 0x21,
ECT_SDO_DOWN_EXP = 0x23,
ECT_SDO_DOWN_INIT_CA = 0x31,
ECT_SDO_UP_REQ = 0x40,
ECT_SDO_UP_REQ_CA = 0x50,
ECT_SDO_SEG_UP_REQ = 0x60,
ECT_SDO_ABORT = 0x80
};
/** CoE Object Description commands */
enum
{
ECT_GET_ODLIST_REQ = 0x01,
ECT_GET_ODLIST_RES = 0x02,
ECT_GET_OD_REQ = 0x03,
ECT_GET_OD_RES = 0x04,
ECT_GET_OE_REQ = 0x05,
ECT_GET_OE_RES = 0x06,
ECT_SDOINFO_ERROR = 0x07
};
/** FoE opcodes */
enum
{
ECT_FOE_READ = 0x01,
ECT_FOE_WRITE,
ECT_FOE_DATA,
ECT_FOE_ACK,
ECT_FOE_ERROR,
ECT_FOE_BUSY
};
/** SoE opcodes */
enum
{
ECT_SOE_READREQ = 0x01,
ECT_SOE_READRES,
ECT_SOE_WRITEREQ,
ECT_SOE_WRITERES,
ECT_SOE_NOTIFICATION,
ECT_SOE_EMERGENCY
};
/** Ethercat registers */
enum
{
ECT_REG_TYPE = 0x0000,
ECT_REG_PORTDES = 0x0007,
ECT_REG_ESCSUP = 0x0008,
ECT_REG_STADR = 0x0010,
ECT_REG_ALIAS = 0x0012,
ECT_REG_DLCTL = 0x0100,
ECT_REG_DLPORT = 0x0101,
ECT_REG_DLALIAS = 0x0103,
ECT_REG_DLSTAT = 0x0110,
ECT_REG_ALCTL = 0x0120,
ECT_REG_ALSTAT = 0x0130,
ECT_REG_ALSTATCODE = 0x0134,
ECT_REG_PDICTL = 0x0140,
ECT_REG_IRQMASK = 0x0200,
ECT_REG_RXERR = 0x0300,
ECT_REG_FRXERR = 0x0308,
ECT_REG_EPUECNT = 0x030C,
ECT_REG_PECNT = 0x030D,
ECT_REG_PECODE = 0x030E,
ECT_REG_LLCNT = 0x0310,
ECT_REG_WDCNT = 0x0442,
ECT_REG_EEPCFG = 0x0500,
ECT_REG_EEPCTL = 0x0502,
ECT_REG_EEPSTAT = 0x0502,
ECT_REG_EEPADR = 0x0504,
ECT_REG_EEPDAT = 0x0508,
ECT_REG_FMMU0 = 0x0600,
ECT_REG_FMMU1 = ECT_REG_FMMU0 + 0x10,
ECT_REG_FMMU2 = ECT_REG_FMMU1 + 0x10,
ECT_REG_FMMU3 = ECT_REG_FMMU2 + 0x10,
ECT_REG_SM0 = 0x0800,
ECT_REG_SM1 = ECT_REG_SM0 + 0x08,
ECT_REG_SM2 = ECT_REG_SM1 + 0x08,
ECT_REG_SM3 = ECT_REG_SM2 + 0x08,
ECT_REG_SM0STAT = ECT_REG_SM0 + 0x05,
ECT_REG_SM1STAT = ECT_REG_SM1 + 0x05,
ECT_REG_SM1ACT = ECT_REG_SM1 + 0x06,
ECT_REG_SM1CONTR = ECT_REG_SM1 + 0x07,
ECT_REG_DCTIME0 = 0x0900,
ECT_REG_DCTIME1 = 0x0904,
ECT_REG_DCTIME2 = 0x0908,
ECT_REG_DCTIME3 = 0x090C,
ECT_REG_DCSYSTIME = 0x0910,
ECT_REG_DCSOF = 0x0918,
ECT_REG_DCSYSOFFSET = 0x0920,
ECT_REG_DCSYSDELAY = 0x0928,
ECT_REG_DCSYSDIFF = 0x092C,
ECT_REG_DCSPEEDCNT = 0x0930,
ECT_REG_DCTIMEFILT = 0x0934,
ECT_REG_DCCUC = 0x0980,
ECT_REG_DCSYNCACT = 0x0981,
ECT_REG_DCSTART0 = 0x0990,
ECT_REG_DCCYCLE0 = 0x09A0,
ECT_REG_DCCYCLE1 = 0x09A4
};
/** standard SDO Sync Manager Communication Type */
#define ECT_SDO_SMCOMMTYPE 0x1c00
/** standard SDO PDO assignment */
#define ECT_SDO_PDOASSIGN 0x1c10
/** standard SDO RxPDO assignment */
#define ECT_SDO_RXPDOASSIGN 0x1c12
/** standard SDO TxPDO assignment */
#define ECT_SDO_TXPDOASSIGN 0x1c13
/** Ethercat packet type */
#define ETH_P_ECAT 0x88A4
/** Error types */
typedef enum
{
EC_ERR_TYPE_SDO_ERROR = 0,
EC_ERR_TYPE_EMERGENCY = 1,
EC_ERR_TYPE_PACKET_ERROR = 3,
EC_ERR_TYPE_SDOINFO_ERROR = 4,
EC_ERR_TYPE_FOE_ERROR = 5,
EC_ERR_TYPE_FOE_BUF2SMALL = 6,
EC_ERR_TYPE_FOE_PACKETNUMBER = 7,
EC_ERR_TYPE_SOE_ERROR = 8,
EC_ERR_TYPE_MBX_ERROR = 9,
EC_ERR_TYPE_FOE_FILE_NOTFOUND = 10,
EC_ERR_TYPE_EOE_INVALID_RX_DATA = 11
} ec_err_type;
/** Struct to retrieve errors. */
typedef struct
{
/** Time at which the error was generated. */
ec_timet Time;
/** Signal bit, error set but not read */
boolean Signal;
/** Slave number that generated the error */
uint16 Slave;
/** CoE SDO index that generated the error */
uint16 Index;
/** CoE SDO subindex that generated the error */
uint8 SubIdx;
/** Type of error */
ec_err_type Etype;
union
{
/** General abortcode */
int32 AbortCode;
/** Specific error for Emergency mailbox */
struct
{
uint16 ErrorCode;
uint8 ErrorReg;
uint8 b1;
uint16 w1;
uint16 w2;
};
};
} ec_errort;
/** Helper macros */
/** Set the count value in the Mailbox header */
#define MBX_HDR_SET_CNT(cnt) ((uint8)((cnt) << 4))
/** Macro to make a word from 2 bytes */
#define MK_WORD(msb, lsb) ((((uint16)(msb)) << 8) | (lsb))
/** Macro to get hi byte of a word */
#define HI_BYTE(w) ((w) >> 8)
/** Macro to get low byte of a word */
#define LO_BYTE(w) ((w) & 0x00ff)
/** Macro to swap hi and low byte of a word */
#define SWAP(w) ((((w) & 0xff00) >> 8) | (((w) & 0x00ff) << 8))
/** Macro to get hi word of a dword */
#define LO_WORD(l) ((l) & 0xffff)
/** Macro to get hi word of a dword */
#define HI_WORD(l) ((l) >> 16)
#define get_unaligned(ptr) \
({ __typeof__(*(ptr)) __tmp; memcpy(&__tmp, (ptr), sizeof(*(ptr))); __tmp; })
#define put_unaligned32(val, ptr) \
(memcpy((ptr), &(val), 4))
#define put_unaligned64(val, ptr) \
(memcpy((ptr), &(val), 8))
#if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN)
#define htoes(A) (A)
#define htoel(A) (A)
#define htoell(A) (A)
#define etohs(A) (A)
#define etohl(A) (A)
#define etohll(A) (A)
#elif !defined(EC_LITTLE_ENDIAN) && defined(EC_BIG_ENDIAN)
#define htoes(A) ((((uint16)(A) & 0xff00) >> 8) | \
(((uint16)(A) & 0x00ff) << 8))
#define htoel(A) ((((uint32)(A) & 0xff000000) >> 24) | \
(((uint32)(A) & 0x00ff0000) >> 8) | \
(((uint32)(A) & 0x0000ff00) << 8) | \
(((uint32)(A) & 0x000000ff) << 24))
#define htoell(A) ((((uint64)(A) & (uint64)0xff00000000000000ULL) >> 56) | \
(((uint64)(A) & (uint64)0x00ff000000000000ULL) >> 40) | \
(((uint64)(A) & (uint64)0x0000ff0000000000ULL) >> 24) | \
(((uint64)(A) & (uint64)0x000000ff00000000ULL) >> 8) | \
(((uint64)(A) & (uint64)0x00000000ff000000ULL) << 8) | \
(((uint64)(A) & (uint64)0x0000000000ff0000ULL) << 24) | \
(((uint64)(A) & (uint64)0x000000000000ff00ULL) << 40) | \
(((uint64)(A) & (uint64)0x00000000000000ffULL) << 56))
#define etohs htoes
#define etohl htoel
#define etohll htoell
#else
#error "Must define one of EC_BIG_ENDIAN or EC_LITTLE_ENDIAN"
#endif
#ifdef __cplusplus
}
#endif
#endif /* _EC_TYPE_H */

108
dependency/x86/third_party/soem/include/soem/nicdrv.h vendored
View File

@ -1,108 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* Headerfile for nicdrv.c
*/
#ifndef _nicdrvh_
#define _nicdrvh_
#ifdef __cplusplus
extern "C" {
#endif
#include <pthread.h>
/** pointer structure to Tx and Rx stacks */
typedef struct
{
/** socket connection used */
int *sock;
/** tx buffer */
ec_bufT (*txbuf)[EC_MAXBUF];
/** tx buffer lengths */
int (*txbuflength)[EC_MAXBUF];
/** temporary receive buffer */
ec_bufT *tempbuf;
/** rx buffers */
ec_bufT (*rxbuf)[EC_MAXBUF];
/** rx buffer status fields */
int (*rxbufstat)[EC_MAXBUF];
/** received MAC source address (middle word) */
int (*rxsa)[EC_MAXBUF];
/** number of received frames */
uint64 rxcnt;
} ec_stackT;
/** pointer structure to buffers for redundant port */
typedef struct
{
ec_stackT stack;
int sockhandle;
/** rx buffers */
ec_bufT rxbuf[EC_MAXBUF];
/** rx buffer status */
int rxbufstat[EC_MAXBUF];
/** rx MAC source address */
int rxsa[EC_MAXBUF];
/** temporary rx buffer */
ec_bufT tempinbuf;
} ecx_redportt;
/** pointer structure to buffers, vars and mutexes for port instantiation */
typedef struct
{
ec_stackT stack;
int sockhandle;
/** rx buffers */
ec_bufT rxbuf[EC_MAXBUF];
/** rx buffer status */
int rxbufstat[EC_MAXBUF];
/** rx MAC source address */
int rxsa[EC_MAXBUF];
/** temporary rx buffer */
ec_bufT tempinbuf;
/** temporary rx buffer status */
int tempinbufs;
/** transmit buffers */
ec_bufT txbuf[EC_MAXBUF];
/** transmit buffer lengths */
int txbuflength[EC_MAXBUF];
/** temporary tx buffer */
ec_bufT txbuf2;
/** temporary tx buffer length */
int txbuflength2;
/** last used frame index */
uint8 lastidx;
/** current redundancy state */
int redstate;
/** pointer to redundancy port and buffers */
ecx_redportt *redport;
pthread_mutex_t getindex_mutex;
pthread_mutex_t tx_mutex;
pthread_mutex_t rx_mutex;
} ecx_portt;
extern const uint16 priMAC[3];
extern const uint16 secMAC[3];
void ec_setupheader(void *p);
int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary);
int ecx_closenic(ecx_portt *port);
void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat);
uint8 ecx_getindex(ecx_portt *port);
int ecx_outframe(ecx_portt *port, uint8 idx, int sock);
int ecx_outframe_red(ecx_portt *port, uint8 idx);
int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout);
int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout);
#ifdef __cplusplus
}
#endif
#endif

222
dependency/x86/third_party/soem/include/soem/osal.h vendored
View File

@ -1,222 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
#ifndef _osal_
#define _osal_
#ifdef __cplusplus
extern "C" {
#endif
#include "osal_defs.h"
#include <stdint.h>
#include <stddef.h>
/* General types */
#ifndef TRUE
#define TRUE 1
#endif
#ifndef FALSE
#define FALSE 0
#endif
typedef uint8_t boolean;
typedef int8_t int8;
typedef int16_t int16;
typedef int32_t int32;
typedef uint8_t uint8;
typedef uint16_t uint16;
typedef uint32_t uint32;
typedef int64_t int64;
typedef uint64_t uint64;
typedef float float32;
typedef double float64;
typedef struct osal_timer
{
ec_timet stop_time;
} osal_timert;
/**
* @brief Returns monotonic time from some unspecified moment in the
* past.
*
* This time must be strictly increasing. It is used for time
* intervals measurement.
*
* @param ts Pointer to an ec_timet structure where the time will be
* stored.
*/
void osal_get_monotonic_time(ec_timet *ts);
/**
* @brief Returns the current time.
*
* This time is used to set the initial EtherCAT network DC time and
* for logging purposes.
*
* @return ec_timet containing the current time.
*/
ec_timet osal_current_time(void);
/**
* @brief Calculates the difference between two timestamps.
*
* @param start Pointer to the start timestamp.
* @param end Pointer to the end timestamp.
* @param diff Pointer to an ec_timet structure where the difference
* will be stored.
*/
void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff);
/**
* @brief Starts the timer with a specified timeout.
*
* @param self Pointer to the timer object.
* @param timeout_usec Timeout in microseconds.
*/
void osal_timer_start(osal_timert *self, uint32 timeout_usec);
/**
* @brief Checks if the timer has expired.
*
* @param self Pointer to the timer object.
* @return True if the timer is expired, false otherwise.
*/
boolean osal_timer_is_expired(osal_timert *self);
/**
* @brief Sleeps for a specified duration in microseconds.
*
* @param usec Duration in microseconds.
* @return 0 on success, -1 on failure.
*/
int osal_usleep(uint32 usec);
/**
* @brief Sleeps until the specified monotonic time.
*
* @param ts Pointer to an ec_timet structure representing the
* absolute time to sleep until.
* @return 0 on success, -1 on failure.
*/
int osal_monotonic_sleep(ec_timet *ts);
/**
* @brief Allocates memory of the specified size.
*
* @param size Size in bytes to allocate.
* @return Pointer to the allocated memory or NULL on failure.
*/
void *osal_malloc(size_t size);
/**
* @brief Frees the allocated memory.
*
* @param ptr Pointer to the memory to free.
*/
void osal_free(void *ptr);
/**
* @brief Creates a new thread.
*
* @param thandle Pointer to the thread handle which will store the
* thread ID.
* @param stacksize Size of the stack for the new thread.
* @param func Pointer to the function to execute in the new thread.
* @param param Pointer to parameters to pass to the thread function.
* @return 1 on success, 0 on failure.
*/
int osal_thread_create(void *thandle, int stacksize, void *func, void *param);
/**
* @brief Creates a new real-time thread.
*
* @param thandle Pointer to the thread handle which will store the
* thread ID.
* @param stacksize Size of the stack for the new thread.
* @param func Pointer to the function to execute in the new thread.
* @param param Pointer to parameters to pass to the thread function.
* @return 1 on success, 0 on failure.
*/
int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param);
/**
* @brief Creates a mutex.
*
* @return Pointer to the created mutex or NULL on failure.
*/
void *osal_mutex_create(void);
/**
* @brief Destroys a mutex.
*
* @param mutex Pointer to the mutex to destroy.
*/
void osal_mutex_destroy(void *mutex);
/**
* @brief Locks the mutex.
*
* @param mutex Pointer to the mutex to lock.
*/
void osal_mutex_lock(void *mutex);
/**
* @brief Unlocks the mutex.
*
* @param mutex Pointer to the mutex to unlock.
*/
void osal_mutex_unlock(void *mutex);
#ifndef osal_timespec_from_usec
#define osal_timespec_from_usec(usec, result) \
do \
{ \
(result)->tv_sec = usec / 1000000; \
(result)->tv_nsec = (usec % 1000000) * 1000; \
} while (0)
#endif
#ifndef osal_timespeccmp
#define osal_timespeccmp(a, b, CMP) \
(((a)->tv_sec == (b)->tv_sec) \
? ((a)->tv_nsec CMP(b)->tv_nsec) \
: ((a)->tv_sec CMP(b)->tv_sec))
#endif
#ifndef osal_timespecadd
#define osal_timespecadd(a, b, result) \
do \
{ \
(result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \
(result)->tv_nsec = (a)->tv_nsec + (b)->tv_nsec; \
if ((result)->tv_nsec >= 1000000000) \
{ \
++(result)->tv_sec; \
(result)->tv_nsec -= 1000000000; \
} \
} while (0)
#endif
#ifndef osal_timespecsub
#define osal_timespecsub(a, b, result) \
do \
{ \
(result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \
(result)->tv_nsec = (a)->tv_nsec - (b)->tv_nsec; \
if ((result)->tv_nsec < 0) \
{ \
--(result)->tv_sec; \
(result)->tv_nsec += 1000000000; \
} \
} while (0)
#endif
#ifdef __cplusplus
}
#endif
#endif

46
dependency/x86/third_party/soem/include/soem/osal_defs.h vendored
View File

@ -1,46 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
#ifndef _osal_defs_
#define _osal_defs_
#ifdef __cplusplus
extern "C" {
#endif
#include <pthread.h>
#include <sys/time.h>
// define if debug printf is needed
#ifdef EC_DEBUG
#include <stdio.h>
#define EC_PRINT printf
#else
#define EC_PRINT(...) \
do \
{ \
} while (0)
#endif
#ifndef OSAL_PACKED
#define OSAL_PACKED_BEGIN
#define OSAL_PACKED __attribute__((__packed__))
#define OSAL_PACKED_END
#endif
#define ec_timet struct timespec
#define OSAL_THREAD_HANDLE pthread_t *
#define OSAL_THREAD_FUNC void
#define OSAL_THREAD_FUNC_RT void
#define osal_mutext pthread_mutex_t
#ifdef __cplusplus
}
#endif
#endif

28
dependency/x86/third_party/soem/include/soem/soem.h vendored
View File

@ -1,28 +0,0 @@
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
/** \file
* \brief
* SOEM main include file
*/
#ifndef _SOEM_H
#define _SOEM_H
#include "soem/ec_options.h"
#include "soem/ec_type.h"
#include "nicdrv.h"
#include "soem/ec_base.h"
#include "soem/ec_main.h"
#include "soem/ec_dc.h"
#include "soem/ec_coe.h"
#include "soem/ec_foe.h"
#include "soem/ec_soe.h"
#include "soem/ec_eoe.h"
#include "soem/ec_config.h"
#include "soem/ec_print.h"
#endif /* _SOEM_H */

BIN
dependency/x86/third_party/soem/lib/libsoem.a vendored
View File

Binary file not shown.

906
protos/cmvr/api/agv_command.proto
View File

@ -1,906 +0,0 @@
/**
* @file agv_command.proto
* @brief AGV 控制相关的 Protobuf 消息定义。
* 涵盖仙工智能（Seer）SRC 系列控制器的主要 API 命令，
* 包括状态查询、电池、位置、地图管理、控制权、运动控制、导航任务等。
* @note 所有命令均遵循仙工协议帧格式，通过 gRPC 服务（AgvService）对外暴露。
*/
syntax = "proto3";
import "cmvr/api/common.proto";
package cmvr.api;
// ============================================================================
// 1. 基本信息查询（命令码 1000, 0x03E8）
// ============================================================================
/**
* @brief AGV 基本信息响应结构。
* @note 对应仙工 API 1000（0x03E8）的响应体，包含设备标识、软硬件版本、网络状态等。
*/
message AgvStatusInfo {
optional string id = 1; ///< AGV 设备唯一 ID
optional string vehicle_id = 2; ///< 车辆编号（如 "agv_001"）
optional string version = 3; ///< 软件版本号
optional string model = 4; ///< 车型型号（如 "SRC-1100"）
optional string dsp_version = 5; ///< DSP 固件版本
optional string current_ip = 6; ///< 当前 IP 地址
optional string mac = 7; ///< MAC 地址
optional int32 rssi = 8; ///< Wi-Fi 信号强度（0~100）
optional int32 ret_code = 9; ///< 返回码，0 表示成功
optional string err_msg = 10; ///< 错误信息（失败时填充）
}
/**
* @brief 获取 AGV 状态命令（请求/反馈）。
* @note 请求携带设备 ID（header.device_id），反馈返回 AgvStatusInfo。
*/
message GetAgvStatusInfoCommand {
message Request {
CommandHeader.Request header = 1; ///< 公共请求头，包含 device_id
}
message Feedback {
CommandHeader.Feedback header = 1; ///< 公共响应头（success/error_message/timestamp）
AgvStatusInfo status = 2; ///< AGV 状态信息
}
}
// ============================================================================
// 2. 电池状态查询（命令码 1007, 0x03EF）
// ============================================================================
/**
* @brief 电池状态响应结构。
* @note 对应仙工 API 1007（0x03EF），支持简单/全量两种模式。
*/
message AgvBatteryStatus {
optional double battery_level = 1; ///< 电池电量（0~1，对应 0%~100%）
optional double battery_temp = 2; ///< 电池温度（摄氏度）
optional bool charging = 3; ///< 是否正在充电
optional double voltage = 4; ///< 当前电压（V）
optional double current = 5; ///< 当前电流（A）
optional double max_charge_voltage = 6; ///< 最大充电电压（-1 表示不支持）
optional double max_charge_current = 7; ///< 最大充电电流（-1 表示不支持）
optional bool manual_charge = 8; ///< 是否连接手动充电器（仅 SRC-2000）
optional bool auto_charge = 9; ///< 是否连接自动充电桩（仅 SRC-2000）
optional int32 battery_cycle = 10; ///< 电池循环次数（从 BMS 读取，可能不准）
optional string battery_user_data = 11; ///< 电池用户自定义数据
optional string extra = 12; ///< 扩展字段（预留）
optional int32 ret_code = 13; ///< 返回码，0 表示成功
optional string create_on = 14; ///< 数据创建时间戳（ISO 8601）
optional string err_msg = 15; ///< 错误信息
}
/**
* @brief 电池状态请求数据。
*/
message RobotStatusBatteryRequestData {
optional bool simple = 1; ///< true=仅关键数据，false=全量数据（默认 false）
}
/**
* @brief 电池状态查询命令。
*/
message RobotStatusBatteryCommand {
message Request {
CommandHeader.Request header = 1; ///< 公共请求头
RobotStatusBatteryRequestData data = 2; ///< 请求参数
}
message Feedback {
CommandHeader.Feedback header = 1; ///< 公共响应头
AgvBatteryStatus status = 2; ///< 电池状态信息
}
}
// ============================================================================
// 3. 机器人位置查询（命令码 1004, 0x03EC）
// ============================================================================
/**
* @brief 机器人位置响应结构。
* @note 对应仙工 API 1004（0x03EC），返回世界坐标系下的位姿。
*/
message AgvRobotLocation {
optional double x = 1; ///< X 坐标（米）
optional double y = 2; ///< Y 坐标（米）
optional double angle = 3; ///< 朝向角（弧度），逆时针为正
optional double confidence = 4; ///< 定位置信度（0~1）
optional string current_station = 5; ///< 当前所在站点 ID（若在站点上）
optional string last_station = 6; ///< 上一个经过的站点 ID
optional int32 loc_method = 7; ///< 定位方式（0=自然轮廓, 1=反光柱, 2=二维码, 3=里程计...）
optional int32 ret_code = 8; ///< 返回码，0 表示成功
optional string create_on = 9; ///< 时间戳
optional string err_msg = 10; ///< 错误信息
}
/**
* @brief 位置查询命令（无请求数据）。
*/
message RobotStatusLocCommand {
message Request {
CommandHeader.Request header = 1; ///< 公共请求头
}
message Feedback {
CommandHeader.Feedback header = 1; ///< 公共响应头
AgvRobotLocation status = 2; ///< 位置信息
}
}
// ============================================================================
// 4. 地图下载（命令码 4011, 0x0FAB）
// ============================================================================
/**
* @brief 地图下载请求数据。
*/
message RobotConfigDownloadMapRequestData {
optional string map_name = 1; ///< 要下载的地图名称（必填）
}
/**
* @brief 地图下载响应结果。
*/
message AgvDownloadMapResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
optional string map_content = 4; ///< 地图 JSON 文件内容（成功时填充）
}
/**
* @brief 地图下载命令。
*/
message RobotConfigDownloadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotConfigDownloadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvDownloadMapResult status = 2;
}
}
// ============================================================================
// 5. 地图状态查询（命令码 1300, 0x0514）
// ============================================================================
/**
* @brief 单个地图文件信息。
*/
message MapFileInfo {
optional string name = 1; ///< 文件名
optional string modified = 2; ///< 最后修改时间
optional int64 size = 3; ///< 文件大小（字节）
}
/**
* @brief 地图状态响应结构。
* @note 对应仙工 API 1300（0x0514），返回当前地图及所有地图列表。
*/
message AgvMapStatus {
optional string current_map = 1; ///< 当前加载的地图名称
optional string current_map_md5 = 2; ///< 当前地图的 MD5 校验值
repeated string maps = 3; ///< 所有地图名称列表
repeated MapFileInfo map_files_info = 4; ///< 每个地图文件的详细信息
optional int32 ret_code = 5; ///< 返回码，0 表示成功
optional string create_on = 6; ///< 时间戳
optional string err_msg = 7; ///< 错误信息
}
/**
* @brief 地图状态查询命令（无请求数据）。
*/
message RobotStatusMapCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvMapStatus status = 2;
}
}
// ============================================================================
// 6. 上传地图（命令码 4010, 0x0FAA）
// ============================================================================
/**
* @brief 上传地图请求数据。
*/
message RobotConfigUploadMapRequestData {
optional string map_content = 1; ///< 完整的地图 JSON 字符串（必填）
}
/**
* @brief 上传地图响应结果。
*/
message AgvUploadMapResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 上传地图命令。
*/
message RobotConfigUploadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotConfigUploadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvUploadMapResult status = 2;
}
}
// ============================================================================
// 7. 抢占控制权（命令码 4005, 0x0FA5）
// ============================================================================
/**
* @brief 抢占控制权请求数据。
*/
message RobotConfigLockRequestData {
optional string nick_name = 1; ///< 抢占者昵称/标识（必填）
}
/**
* @brief 抢占控制权响应结果。
*/
message AgvLockResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 抢占控制权命令。
*/
message RobotConfigLockCommand {
message Request {
CommandHeader.Request header = 1;
RobotConfigLockRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvLockResult status = 2;
}
}
// ============================================================================
// 8. 查询当前控制权所有者（命令码 1060, 0x0424）
// ============================================================================
/**
* @brief 当前控制权所有者响应结构。
* @note 对应仙工 API 1060（0x0424）。
*/
message AgvCurrentLockStatus {
optional bool locked = 1; ///< 是否被锁定（有持有者）
optional string ip = 2; ///< 持有者 IP 地址
optional int32 port = 3; ///< 持有者端口
optional uint32 type = 4; ///< 持有者类型（0=default, 2=roboshop, 0xDD=srd）
optional string nick_name = 5; ///< 持有者昵称
optional int64 time_t = 6; ///< 锁定时间戳（Unix 秒）
optional string desc = 7; ///< 描述信息
optional int32 ret_code = 8; ///< 返回码，0 表示成功
optional string create_on = 9; ///< 时间戳
optional string err_msg = 10; ///< 错误信息
}
/**
* @brief 查询控制权命令（无请求数据）。
*/
message RobotStatusCurrentLockCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvCurrentLockStatus status = 2;
}
}
// ============================================================================
// 9. 开环速度运动控制（命令码 2010, 0x07DA）
// ============================================================================
/**
* @brief 开环速度运动控制请求数据。
* @note 命令码 2010（0x07DA）。下发后会强制取消当前自动导航任务。
* 多舵轮设备仅 vx/vy/w 生效；steer/real_steer 仅单舵轮设备有效。
* duration = -1 表示持续运行直到下一次指令。
*/
message RobotMotionControlRequestData {
optional double vx = 1; ///< X 方向线速度（m/s）
optional double vy = 2; ///< Y 方向线速度（m/s）
optional double w = 3; ///< 角速度（rad/s），逆时针为正
optional double steer = 4; ///< 舵轮转向角（单舵轮，单位 rad）
optional double real_steer = 5; ///< 实际舵轮角度（单舵轮，优先级高于 steer）
optional int64 duration = 6; ///< 持续时间（ms），-1 表示一直运行
}
/**
* @brief 开环运动控制响应结果。
*/
message RobotMotionControlResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 开环运动控制命令。
*/
message RobotMotionControlCommand {
message Request {
CommandHeader.Request header = 1;
RobotMotionControlRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotMotionControlResult status = 2;
}
}
// ============================================================================
// 10. 切换载入地图（命令码 2022, 0x07E6）
// ============================================================================
/**
* @brief 切换地图请求数据。
* @note 命令码 2022（0x07E6）。目标地图必须已存在于机器人中。
*/
message RobotLoadMapRequestData {
optional string map_name = 1; ///< 目标地图名称（必填，仅字母数字-_）
}
/**
* @brief 切换地图响应结果。
*/
message RobotLoadMapResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 切换地图命令。
*/
message RobotLoadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotLoadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotLoadMapResult status = 2;
}
}
// ============================================================================
// 11. 查询地图载入状态（命令码 1022, 0x03FE）
// ============================================================================
/**
* @brief 地图载入状态响应。
* @note 命令码 1022（0x03FE）。loadmap_status: 0=失败, 1=成功, 2=载入中。
* 载入中（2）时禁止执行重定位操作。
*/
message RobotQueryLoadMapStatusResult {
optional int32 loadmap_status = 1; ///< 0=失败, 1=成功, 2=载入中
optional int32 ret_code = 2; ///< 返回码，0 表示成功
optional string create_on = 3; ///< 时间戳
optional string err_msg = 4; ///< 错误信息
}
/**
* @brief 查询地图载入状态命令（无请求数据）。
*/
message RobotQueryLoadMapStatusCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotQueryLoadMapStatusResult status = 2;
}
}
// ============================================================================
// 12. 查询站点列表（命令码 1301, 0x0515）
// ============================================================================
/**
* @brief 单个站点信息。
*/
message StationItem {
optional string id = 1; ///< 站点唯一 ID
optional string type = 2; ///< 站点类型（如 "LocationMark", "ChargePoint"）
optional double x = 3; ///< X 坐标（米）
optional double y = 4; ///< Y 坐标（米）
optional double r = 5; ///< 朝向角（弧度）
optional string desc = 6; ///< 描述信息
optional string executor = 7; ///< 执行器信息
optional string prepoint = 8; ///< 前置点 ID
optional string recfile = 9; ///< 识别文件路径
optional bool spin = 10; ///< 是否允许自旋
optional bool use_down_pgv = 11; ///< 是否使用下视 PGV
}
/**
* @brief 站点列表响应结果。
*/
message QueryStationListResult {
repeated StationItem stations = 1; ///< 站点列表
optional int32 ret_code = 2; ///< 返回码，0 表示成功
optional string create_on = 3; ///< 时间戳
optional string err_msg = 4; ///< 错误信息
}
/**
* @brief 查询站点列表命令（无请求数据）。
*/
message QueryStationListCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
QueryStationListResult status = 2;
}
}
// ============================================================================
// 13. 指定路径导航（命令码 3066, 0x0BFA）
// ============================================================================
/**
* @brief 单段导航任务单元。
* @note 用于 3066 命令的 move_task_list 数组。
* source_id 和 id 之间必须有直接路径，不可跳点。
*/
message MoveTaskItem {
optional string task_id = 1; ///< 任务唯一 ID（必填）
optional string source_id = 2; ///< 起始站点 ID（必填）
optional string id = 3; ///< 目标站点 ID（必填）
optional string operation = 4; ///< 到达后执行的动作（如 "JackLoad"）
optional double jack_height = 5; ///< 顶升高度（米）
}
/**
* @brief 指定路径导航请求数据。
*/
message RobotGoTargetListRequestData {
repeated MoveTaskItem move_task_list = 1; ///< 导航任务序列（顺序执行）
}
/**
* @brief 指定路径导航响应结果。
* @note ret_code=0 仅表示指令被接收，不表示执行完成。
*/
message RobotGoTargetListResult {
optional int32 ret_code = 1; ///< 返回码，0 表示成功接收
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 指定路径导航命令。
*/
message RobotGoTargetListCommand {
message Request {
CommandHeader.Request header = 1;
RobotGoTargetListRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotGoTargetListResult status = 2;
}
}
// ============================================================================
// 14. 查询当前实时导航状态（命令码 1020, 0x03FC）
// ============================================================================
/**
* @brief 1020 命令请求数据。
*/
message RobotStatusTaskReqData {
optional bool simple = 1; ///< true=仅返回 task_status，false=全量
}
/**
* @brief 导航任务中的料箱信息。
*/
message NavContainerItem {
optional string container_name = 1; ///< 料箱名称
optional string desc = 2; ///< 描述
optional string goods_id = 3; ///< 绑定的货物 ID
optional bool has_goods = 4; ///< 是否已绑定货物
}
/**
* @brief 1020 命令响应数据。
*/
message RobotStatusTaskResData {
optional int32 task_status = 1; ///< 0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED
optional int32 task_type = 2; ///< 0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点, 7=平动转动
optional string target_id = 3; ///< 目标站点 ID（task_type 为 2/3 时）
repeated double target_point = 4; ///< 目标坐标 [x, y, r]（task_type 为 1 时）
repeated string finished_path = 5; ///< 已经过的站点列表
repeated string unfinished_path = 6; ///< 尚未经过的站点列表
optional string move_status_info = 7; ///< 附加信息（如错误描述）
repeated NavContainerItem containers = 8; ///< 料箱信息
optional int32 ret_code = 9; ///< 返回码，0 表示成功
optional string create_on = 10; ///< 时间戳
optional string err_msg = 11; ///< 错误信息
}
/**
* @brief 查询当前导航状态命令。
*/
message RobotStatusTaskCurrentCommand {
message Request {
CommandHeader.Request header = 1;
RobotStatusTaskReqData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotStatusTaskResData data = 2;
}
}
// ============================================================================
// 15. 批量查询任务状态（命令码 1110, 0x0456）
// ============================================================================
/**
* @brief 1110 命令请求数据。
*/
message QueryTaskStatusPackageReqData {
repeated string task_ids = 1; ///< 要查询的任务 ID 列表；为空则查所有未完成 + 最近一条完成
}
/**
* @brief 单条任务状态项。
*/
message SingleTaskStatusItem {
optional string task_id = 1; ///< 任务 ID
optional int32 status = 2; ///< 状态码（同 task_status）
optional int32 type = 3; ///< 任务类型（同 task_type）
}
/**
* @brief 任务状态包（包含最近完成和所有未完成的任务）。
*/
message TaskStatusPackage {
optional string closest_target = 1; ///< 最近站点 ID
optional string source_name = 2; ///< 起点站点名称
optional string target_name = 3; ///< 终点站点名称
optional double percentage = 4; ///< 任务进度百分比（0~100）
optional double distance = 5; ///< 剩余距离（米）
optional string info = 6; ///< 附加信息
repeated SingleTaskStatusItem task_status_list = 7; ///< 所有任务状态列表
}
/**
* @brief 1110 命令响应数据。
*/
message QueryTaskStatusPackageResData {
optional TaskStatusPackage task_status_package = 1; ///< 任务状态包
optional int32 ret_code = 2; ///< 返回码，0 表示成功
optional string create_on = 3; ///< 时间戳
optional string err_msg = 4; ///< 错误信息
}
/**
* @brief 批量查询任务状态命令。
*/
message RobotStatusTaskPackageCommand {
message Request {
CommandHeader.Request header = 1;
QueryTaskStatusPackageReqData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
QueryTaskStatusPackageResData data = 2;
}
}
// ============================================================================
// 16. 单点站点自动规划导航（命令码 3051, 0x0BEB）
// ============================================================================
/**
* @brief DI（数字输入）点位。
*/
message DIItem {
int32 id = 1; ///< DI 编号
bool status = 2; ///< 状态（true=高电平, false=低电平）
}
/**
* @brief DO（数字输出）点位。
*/
message DOItem {
int32 id = 1; ///< DO 编号
bool status = 2; ///< 状态（true=高电平, false=低电平）
}
/**
* @brief 音频播放参数。
*/
message SoundArgs {
optional string name = 1; ///< 音频文件名
optional int32 loop = 2; ///< 循环次数（0=播放一次, 1=循环）
optional int32 stop = 3; ///< 1=停止播放
}
/**
* @brief WaitDI 动作参数。
*/
message WaitDIArgs {
repeated DIItem DI = 1; ///< 需要等待的 DI 条件列表
optional double timeout = 2; ///< 超时时间（秒），0 表示无限等待
}
/**
* @brief SetDO 动作参数。
*/
message SetDOArgs {
repeated DOItem do_list = 1; ///< 要设置的 DO 列表
}
/**
* @brief PGV（二维码）二次定位参数。
*/
message PgvParam {
optional bool use_pgv = 1; ///< 是否使用上视 PGV
optional bool use_down_pgv = 2; ///< 是否使用下视 PGV
optional double pgv_adjust_dist = 3; ///< 最大调整半径（米）
optional double pgv_adjust_cx = 4; ///< 二维码坐标系 X 偏移（米）
optional double pgv_adjust_cy = 5; ///< 二维码坐标系 Y 偏移（米）
optional double pgv_x_adjust = 6; ///< 仅调整 X 方向误差（米）
}
/**
* @brief 自由导航目标点（坐标 + 朝向）。
*/
message FreeGoPoint {
double x = 1; ///< X 坐标（米）
double y = 2; ///< Y 坐标（米）
double theta = 3; ///< 朝向角（弧度）
}
/**
* @brief 脚本自定义参数。
*/
message ScriptArgs {
map<string, string> str_kv = 1; ///< 字符串键值对
map<string, double> num_kv = 2; ///< 数字键值对
repeated DOItem do_list = 3; ///< DO 列表
repeated DIItem di_list = 4; ///< DI 列表
}
/**
* @brief 3051 命令请求数据（单点站点自动规划导航）。
* @warning 严禁用于多车调度场景，仅限单车测试/验证。
* 下发此任务会取消当前正在执行的任务（不排队）。
* 成功下发后会自动清除指定的 warning/error 报错码。
* 自由导航（freego）仅支持双轮差速底盘。
*/
message RobotGoTargetReqData {
// -------- 必填字段 --------
string source_id = 1; ///< 起始站点 ID（"SELF_POSITION" 表示当前位置）
string id = 2; ///< 目标站点 ID（"SELF_POSITION" 表示原地执行 operation）
optional string task_id = 3; ///< 任务 ID（建议提供）
// -------- 运动通用参数 --------
optional double angle = 4; ///< 目标点朝向角（弧度）
optional string method = 5; ///< "forward"（正走）或 "backward"（倒走）
optional double max_speed = 6; ///< 最大线速度（m/s），0 使用默认
optional double max_wspeed = 7; ///< 最大角速度（rad/s）
optional double max_acc = 8; ///< 最大加速度（m/s²）
optional double max_wacc = 9; ///< 最大角加速度（rad/s²）
optional int64 duration = 10; ///< 到达后等待时间（毫秒）
optional int32 orientation = 11; ///< 全向车保持的角度（暂不使用）
optional bool spin = 12; ///< 是否随动（托盘旋转）
optional int64 delay = 13; ///< 延迟结束导航状态（毫秒），缺省 0
optional int32 start_rot_dir = 14; ///< 起步旋转方向：-1=顺时针, 0=就近, 1=逆时针
optional int32 end_rot_dir = 15; ///< 到点旋转方向：-1=顺时针, 0=就近, 1=逆时针
optional double reach_dist = 16; ///< 到点位置精度（米）
optional double reach_angle = 17; ///< 到点角度精度（弧度）
optional string skill_name = 18; ///< 技能名称："Action" 或 "GotoSpecifiedPose"
// -------- PGV --------
optional PgvParam pgv = 19; ///< 二次定位参数
// -------- 设备动作 --------
optional string operation = 20; ///< 操作类型（JackLoad/ForkUnload/RollerLoad/HookLoad/WaitDI/SetDO/sound/Script 等）
optional double jack_height = 21; ///< 顶升高度（米）
optional double start_height = 22; ///< 货叉起步前举升高度（米）
optional double end_height = 23; ///< 货叉到点后举升高度（米）
optional double fork_mid_height = 24; ///< 货叉行走过程中举升高度（米）
optional double fork_dist = 25; ///< 货叉前移距离（米）
optional string direction = 26; ///< 方向："left"/"right"/"front"/"back"
optional bool recognize = 27; ///< 是否启用视觉识别
optional string recfile = 28; ///< 识别文件路径（如 "shelf/s0002.shelf"）
// -------- 音频 --------
optional SoundArgs sounds_args = 29; ///< 音频参数
// -------- WaitDI / SetDO --------
optional WaitDIArgs wait_di_args = 30; ///< WaitDI 参数
optional SetDOArgs set_do_args = 31; ///< SetDO 参数
// -------- 脚本 --------
optional string script_name = 32; ///< 脚本文件名
optional ScriptArgs script_args = 33; ///< 脚本参数
optional int32 script_stage = 34; ///< 执行阶段：0=导航前, 1=导航中, 2=导航后, 3=脚本控制导航
// -------- 原地旋转（GoByOdometer） --------
optional double move_angle = 35; ///< 旋转角度（弧度）
optional double speed_w = 36; ///< 旋转角速度（rad/s）
optional int32 loc_mode = 37; ///< 定位模式：1=激光定位, 0=里程定位
// -------- 自由导航（仅双轮差速） --------
optional FreeGoPoint freego = 38; ///< 自由导航目标（若提供则忽略 id 字段）
}
/**
* @brief 3051 命令响应数据。
*/
message RobotGoTargetResData {
optional int32 ret_code = 1; ///< 返回码，0 表示成功接收
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 单点导航命令。
*/
message RobotGoTargetCommand {
message Request {
CommandHeader.Request header = 1;
RobotGoTargetReqData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotGoTargetResData data = 2;
}
}
// ============================================================================
// 17. 停止开环运动（命令码 2000, 0x07D0）
// ============================================================================
/**
* @brief 停止开环运动请求数据（无业务字段）。
* @note 命令码 2000（0x07D0）。下发零速度指令，终止当前持续运动。
*/
message RobotControlStopRequestData
{
}
/**
* @brief 停止开环运动响应结果。
*/
message RobotControlStopResult
{
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
/**
* @brief 停止开环运动命令。
*/
message RobotControlStopCommand
{
message Request
{
CommandHeader.Request header = 1;
RobotControlStopRequestData data = 2;
}
message Feedback
{
CommandHeader.Feedback header = 1;
RobotControlStopResult status = 2;
}
}
// ============================================================================
// 18. 暂停/继续/取消导航任务（命令码 3001/3002/3003）
// ============================================================================
/**
* @brief 暂停当前导航任务（命令码 3001, 0x0BB9）。
* @note 请求无业务数据。
*/
message RobotTaskPauseCommand {
message Request {
CommandHeader.Request header = 1;
// 无 data 字段
}
message Feedback {
CommandHeader.Feedback header = 1;
/**
* @brief 暂停结果。
*/
message Result {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
Result status = 2;
}
}
/**
* @brief 继续当前导航任务（命令码 3002, 0x0BBA）。
* @note 请求无业务数据。
*/
message RobotTaskResumeCommand {
message Request {
CommandHeader.Request header = 1;
// 无 data 字段
}
message Feedback {
CommandHeader.Feedback header = 1;
/**
* @brief 继续结果。
*/
message Result {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
Result status = 2;
}
}
/**
* @brief 取消当前导航任务（命令码 3003, 0x0BBB）。
* @note 请求无业务数据。
*/
message RobotTaskCancelCommand {
message Request {
CommandHeader.Request header = 1;
// 无 data 字段
}
message Feedback {
CommandHeader.Feedback header = 1;
/**
* @brief 取消结果。
*/
message Result {
optional int32 ret_code = 1; ///< 返回码，0 表示成功
optional string create_on = 2; ///< 时间戳
optional string err_msg = 3; ///< 错误信息
}
Result status = 2;
}
}

74
protos/cmvr/api/agv_service.proto
View File

@ -1,74 +0,0 @@
/**
* @file agv_service.proto
* @brief 定义AGV服务的gRPC接口，聚合所有AGV相关命令。
* 服务端实现类为 gRPCAGVServiceImpl。
*/
syntax = "proto3";
import "cmvr/api/agv_command.proto";
package cmvr.api;
service AgvService {
// 基本控制
rpc GetStatusInfo(GetAgvStatusInfoCommand.Request) returns (GetAgvStatusInfoCommand.Feedback);
rpc GetBatteryStatus(RobotStatusBatteryCommand.Request) returns (RobotStatusBatteryCommand.Feedback);
rpc GetRobotLocation(RobotStatusLocCommand.Request) returns (RobotStatusLocCommand.Feedback);
rpc RobotConfigDownloadMap(RobotConfigDownloadMapCommand.Request) returns (RobotConfigDownloadMapCommand.Feedback);
rpc GetMapStatus(RobotStatusMapCommand.Request) returns (RobotStatusMapCommand.Feedback);
rpc RobotConfigUploadMap(RobotConfigUploadMapCommand.Request) returns (RobotConfigUploadMapCommand.Feedback);
// 抢占机器人控制权
rpc RobotConfigLock(RobotConfigLockCommand.Request) returns (RobotConfigLockCommand.Feedback);
// 查询当前控制权所有者
rpc GetCurrentLockStatus(RobotStatusCurrentLockCommand.Request) returns (RobotStatusCurrentLockCommand.Feedback);
// 开环速度运动控制
rpc RobotMotionControl(RobotMotionControlCommand.Request) returns (RobotMotionControlCommand.Feedback);
// 切换载入地图 命令码2022
rpc RobotLoadMap(RobotLoadMapCommand.Request) returns (RobotLoadMapCommand.Feedback);
// 查询地图载入状态 命令码1022
rpc QueryLoadMapStatus(RobotQueryLoadMapStatusCommand.Request) returns (RobotQueryLoadMapStatusCommand.Feedback);
// 查询当前地图站点列表 命令码1301
rpc QueryStationList(QueryStationListCommand.Request) returns (QueryStationListCommand.Feedback);
// 指定路径导航 命令码3066
rpc RobotGoTargetList(RobotGoTargetListCommand.Request) returns (RobotGoTargetListCommand.Feedback);
// 查询当前导航状态 1020 robot_status_task_req
// 1020 查询当前实时导航 robot_status_task_req
rpc RobotStatusTaskCurrent(RobotStatusTaskCurrentCommand.Request) returns (RobotStatusTaskCurrentCommand.Feedback);
// 批量查询任务状态 1110 robot_status_task_status_package_req
rpc RobotStatusTaskPackage(RobotStatusTaskPackageCommand.Request) returns (RobotStatusTaskPackageCommand.Feedback);
// 3051 单点站点自动规划导航 robot_task_gotarget_req 0x0BEB
rpc RobotGoTarget(RobotGoTargetCommand.Request) returns (RobotGoTargetCommand.Feedback);
// 0x07D0 停止开环运动
rpc RobotControlStop(RobotControlStopCommand.Request) returns (RobotControlStopCommand.Feedback);
// 暂停当前导航任务 命令码 3001 (0x0BB9)
rpc RobotTaskPause(RobotTaskPauseCommand.Request) returns (RobotTaskPauseCommand.Feedback);
// 继续当前导航任务 命令码 3002 (0x0BBA)
rpc RobotTaskResume(RobotTaskResumeCommand.Request) returns (RobotTaskResumeCommand.Feedback);
// 取消当前导航任务 命令码 3003 (0x0BBB)
rpc RobotTaskCancel(RobotTaskCancelCommand.Request) returns (RobotTaskCancelCommand.Feedback);
}

20
protos/cmvr/config/agv_config/agv_config.proto
View File

@ -1,20 +0,0 @@
// camera_config.proto - 所有配置都在这里
syntax = "proto3";
package cmvr.config;
message AGVsrc1100Config{
string id = 1;
string ip = 2;
bool enable = 3;
int32 port_status = 4; // 19204 状态查询
int32 port_control = 5; // 19205 控制API
int32 port_nav = 6; // 19206 导航API
int32 port_config = 7; // 19207 配置API
int32 port_other = 8; // 19210 其他API
int32 port_push = 9; // 19301 推送API
}
message AGVConfig{
repeated AGVsrc1100Config src1100_agvs = 1;
}

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

@ -1,44 +0,0 @@
// biohead.proto
syntax = "proto3";
package cmvr.config;
// 眉毛配置
message EyeBrowConfig {
string serial = 1; // 如 "64:0~3"
string offest = 2; // 偏移量，如 "90 90 90 90"
string jLmtUp = 3; // 上限，如 "90 170 155 110"
string jLmtLow = 4; // 下限，如 "20 77 90 20"
}
// 眼睛配置
message EyeConfig {
string serial = 1; // 如 "64:4~9"
string offest = 2; // 偏移量，如 "90 90 90 90 90 90"
string jLmtUp = 3; // 上限，如 "90 150 165 90 120 115"
string jLmtLow = 4; // 下限，如 "20 90 90 25 70 75"
}
// 嘴巴配置
message MouthConfig {
string serial = 1; // 如 "65:0~9"
string offest = 2; // 偏移量，如 "90 90 90 90 90 90 90 90 90 90"
string jLmtUp = 3; // 上限，如 "150 110 130 140 100 105 110 125 90 95"
string jLmtLow = 4; // 下限，如 "70 30 80 80 65 55 45 80 85 90"
}
// BioHead配置
message BioHeadConfig {
string id = 1; // 设备ID，如 "bio_head"
string serial = 2; // 串口路径，如 "/dev/ttyUSB0"
int32 ctrlFreq = 3; // 控制频率，如 50
EyeBrowConfig eyebrow = 4; // 眉毛配置
EyeConfig eye = 5; // 眼睛配置
MouthConfig mouth = 6; // 嘴巴配置
bool enable = 7; // 是否启用
}
message HeadConfig {
repeated BioHeadConfig bioheads = 3;
}

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

@ -1,22 +0,0 @@
syntax = "proto3";
package cmvr.config;
// 文件夹监控配置
message FolderConfig {
string fileDir = 1; // 文件夹路径
int32 maxVolume = 2; // 最大容量（MB）
}
// 磁盘监控配置
message DiskMonitorConfig {
string id = 1; // 监控器ID，如 "file_monitor"
int32 freq = 2; // 监控频率（Hz）
repeated FolderConfig folders = 3; // 要监控的文件夹列表
bool enable = 4; // 是否启用
}
// 监控管理器配置
message MonitorManagerConfig {
DiskMonitorConfig disk_monitor = 1; // 磁盘监控配置
}

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

@ -1,88 +0,0 @@
syntax = "proto3";
package cmvr.config;
// 电机配置
message MotorConfig {
int32 id = 1; // 电机ID
string jointName = 2; // 关节名称
float limitQLb = 3; // 位置下限
float limitQUb = 4; // 位置上限
float limitQd = 5; // 速度限制
}
// 通用的CAN设备配置（手臂、头部、腰部都可以复用）
message CanDeviceConfig {
string id = 1; // CAN ID
string devId = 2; // 设备ID
int32 channelId = 3; // 通道ID
bool enable = 4; // 是否启用
string toolFrame = 5; // 工具坐标系（手臂需要，头/腰可选）
repeated MotorConfig motors = 6; // 电机列表
}
// CAN管理器配置
message CanManagerConfig {
string id = 1; // CAN管理器ID
string devId = 2; // 设备ID
CanDeviceConfig left_arm = 3; // 左臂CAN
CanDeviceConfig right_arm = 4; // 右臂CAN
CanDeviceConfig head = 5; // 头部CAN
CanDeviceConfig waist = 6; // 腰部CAN
}
// 人形机器人配置
message HumanRobotConfig {
string id = 1; // 机器人ID，如 "hc01"
int32 dof = 2; // 自由度，如 14
string urdf = 3; // URDF文件路径
string baseLink = 4; // 基座link名称，如 "PELVIS_S"
string jointNames = 5; // 关节名称列表，逗号分隔
string linkNames = 6; // link名称列表，逗号分隔
int32 bufferSize = 7; // 缓冲区大小，如 50
bool verbose = 8; // 是否详细输出
int32 updFreq = 9; // 更新频率，如 500
CanManagerConfig can_manager = 10; // CAN管理器配置
bool enable = 11; // 是否启用
}
// EtherCAT主站配置
message EthercatMasterConfig {
string interface = 1; // 网络接口，如 "eth0"
int32 cycle_time_us = 2; // 周期时间（微秒），默认1000
bool enable = 3; // 是否启用
}
// EtherCAT电机驱动配置
message EthercatMotorConfig {
int32 slave = 1; // 从站ID
string jointName = 2; // 关节名称
float limitQLb = 3; // 位置下限
float limitQUb = 4; // 位置上限
float limitQd = 5; // 速度限制
}
// EtherCAT机器人配置
message EthercatRobotConfig {
string id = 1; // 机器人ID
int32 dof = 2; // 自由度
string urdf = 3; // URDF文件路径
string baseLink = 4; // 基座link名称
string jointNames = 5; // 关节名称列表，逗号分隔
string linkNames = 6; // link名称列表，逗号分隔
int32 bufferSize = 7; // 缓冲区大小
bool verbose = 8; // 是否详细输出
int32 updFreq = 9; // 更新频率（Hz）
bool enable = 10; // 是否启用
EthercatMasterConfig master = 11; // EtherCAT主站配置
repeated EthercatMotorConfig motors = 12; // 电机配置列表
}
// 主配置
message RobotConfig {
repeated HumanRobotConfig humanrobots = 1;
repeated EthercatRobotConfig ethercat_robots = 2; // 新增的EtherCAT机器人
}

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

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

1
request.txt
View File

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