diff --git a/CMakeLists.txt b/CMakeLists.txt index 77cc732c..9e100d3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/cmvr-es/common/config/agv_config/agv_config.pb.txt b/cmvr-es/common/config/agv_config/agv_config.pb.txt deleted file mode 100644 index e9c35fe1..00000000 --- a/cmvr-es/common/config/agv_config/agv_config.pb.txt +++ /dev/null @@ -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 -} diff --git a/cmvr-es/common/config/biohead_config/biohead_config.pb.txt b/cmvr-es/common/config/biohead_config/biohead_config.pb.txt deleted file mode 100644 index b2880e64..00000000 --- a/cmvr-es/common/config/biohead_config/biohead_config.pb.txt +++ /dev/null @@ -1,27 +0,0 @@ -bioheads { - id: "bio_head" - serial: "/dev/ttyUSB0" - ctrlFreq: 50 - enable: false - - eyebrow { - serial: "64:0~3" - offest: "90 90 90 90" - jLmtUp: "90 170 155 110" - jLmtLow: "20 77 90 20" - } - - eye { - serial: "64:4~9" - offest: "90 90 90 90 90 90" - jLmtUp: "90 150 165 90 120 115" - jLmtLow: "20 90 90 25 70 75" - } - - mouth { - serial: "65:0~9" - offest: "90 90 90 90 90 90 90 90 90 90" - jLmtUp: "150 110 130 140 100 105 110 125 90 95" - jLmtLow: "70 30 80 80 65 55 45 80 85 90" - } -} \ No newline at end of file diff --git a/cmvr-es/common/config/camera_config/camera_config.pb.txt b/cmvr-es/common/config/camera_config/camera_config.pb.txt index f35f4018..18b02c30 100644 --- a/cmvr-es/common/config/camera_config/camera_config.pb.txt +++ b/cmvr-es/common/config/camera_config/camera_config.pb.txt @@ -27,7 +27,7 @@ realsense_cameras { align_mode: ALIGN_MODE_COLOR buffer_size: 30 sync: false - enable: false + enable: true } realsense_cameras { diff --git a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt index 4a38bd2f..e6783dd2 100644 --- a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt +++ b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt @@ -1,6 +1,6 @@ rh56dftp_dexhands { id: "hand1" - ip: "192.168.1.223" + ip: "192.168.1.213" port: 6000 poll_interval_ms: 10 enable: false @@ -11,7 +11,7 @@ rh56dftp_dexhands { ip: "192.168.1.224" port: 6000 poll_interval_ms: 10 - enable: false + enable: true } px_6ax_gen3 { @@ -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 } diff --git a/cmvr-es/common/config/monitor_config/monitor_config.pb.txt b/cmvr-es/common/config/monitor_config/monitor_config.pb.txt deleted file mode 100644 index d5350f3c..00000000 --- a/cmvr-es/common/config/monitor_config/monitor_config.pb.txt +++ /dev/null @@ -1,25 +0,0 @@ -# 监控管理器配置 -monitor_manager { - disk_monitor { - id: "file_monitor" - freq: 1 - enable: false - - folders { - fileDir: "/home/share/assets/audio" - maxVolume: 1000 - } - folders { - fileDir: "/home/share/assets/image" - maxVolume: 1000 - } - folders { - fileDir: "/home/share/assets/video" - maxVolume: 1000 - } - folders { - fileDir: "../log" - maxVolume: 1000 - } - } -} \ No newline at end of file diff --git a/cmvr-es/common/config/robot_config/robot_config.pb.txt b/cmvr-es/common/config/robot_config/robot_config.pb.txt deleted file mode 100644 index 9e2a9327..00000000 --- a/cmvr-es/common/config/robot_config/robot_config.pb.txt +++ /dev/null @@ -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 - } -} \ No newline at end of file diff --git a/cmvr-es/common/config/server_config/server_config.pb.txt b/cmvr-es/common/config/server_config/server_config.pb.txt deleted file mode 100644 index 78599a06..00000000 --- a/cmvr-es/common/config/server_config/server_config.pb.txt +++ /dev/null @@ -1,3 +0,0 @@ -grpc_server { - port: "50052" -} \ No newline at end of file diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h index e7ae3404..7d189128 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/cmvr-es/common/utils/config_helper/include/config_helper.h @@ -10,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 diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h index 82c22b5f..5d58fb9e 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/cmvr-es/common/utils/config_helper/include/config_setting.h @@ -6,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); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 041b4885..9e44092d 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -88,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"); diff --git a/cmvr-es/common/utils/ffmpeg/include/FrameData.h b/cmvr-es/common/utils/ffmpeg/include/FrameData.h deleted file mode 100644 index cd69682a..00000000 --- a/cmvr-es/common/utils/ffmpeg/include/FrameData.h +++ /dev/null @@ -1,53 +0,0 @@ -// FrameData.h -#pragma once - -#include - -extern "C" { -#include -#include -} - -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 \ No newline at end of file diff --git a/cmvr-es/data_center/CMakeLists.txt b/cmvr-es/data_center/CMakeLists.txt index ff66818d..d0fda580 100644 --- a/cmvr-es/data_center/CMakeLists.txt +++ b/cmvr-es/data_center/CMakeLists.txt @@ -11,7 +11,6 @@ target_link_libraries(data_center PRIVATE cmvr_es::utils cmvr_es::device::canbus cmvr_es::device::ti5motor - cmvr_es::device::eyoumotor protobuf glog ) diff --git a/cmvr-es/data_center/include/motors_info.h b/cmvr-es/data_center/include/motors_info.h index ac892cfc..56c2de04 100644 --- a/cmvr-es/data_center/include/motors_info.h +++ b/cmvr-es/data_center/include/motors_info.h @@ -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; diff --git a/cmvr-es/data_center/src/motors_info.cpp b/cmvr-es/data_center/src/motors_info.cpp index 7cfa7855..4b6a087c 100644 --- a/cmvr-es/data_center/src/motors_info.cpp +++ b/cmvr-es/data_center/src/motors_info.cpp @@ -3,8 +3,6 @@ // #include "data_center/include/motors_info.h" - -#include "motor/eyou_motor/eyou_motor.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" #include "motor/ti5_motor/ti5_motor.h" @@ -96,10 +94,7 @@ void MotorsInfo::init(const XmlNode &cfg) { }; // 创建 MotorManager - if (!motor_manager_) - { - motor_manager_ = std::make_shared(); - } + motor_manager_ = std::make_shared(); std::vector > 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 l_motors_cfg; - std::vector r_motors_cfg; - std::vector waist_motors_cfg; - std::vector head_motors_cfg; - - // 左臂 - if (left_arm_enabled_) { - const auto& left_arm = can_manager.left_arm(); - for (const auto& motor : left_arm.motors()) { - l_motors_cfg.push_back(motor); - } - l_can_client_ = std::make_shared(left_arm); - l_can_sender_ = std::make_shared>(); - l_can_receiver_ = std::make_shared>(); - l_message_manager_ = std::make_shared>(); - } - - // 右臂 - if (right_arm_enabled_) { - const auto& right_arm = can_manager.right_arm(); - for (const auto& motor : right_arm.motors()) { - r_motors_cfg.push_back(motor); - } - r_can_client_ = std::make_shared(right_arm); - r_can_sender_ = std::make_shared>(); - r_can_receiver_ = std::make_shared>(); - r_message_manager_ = std::make_shared>(); - } - - // 腰部 - if (waist_enabled_) { - const auto& waist = can_manager.waist(); - for (const auto& motor : waist.motors()) { - waist_motors_cfg.push_back(motor); - } - waist_can_client_ = std::make_shared(waist); - waist_can_sender_ = std::make_shared>(); - waist_can_receiver_ = std::make_shared>(); - waist_message_manager_ = std::make_shared>(); - } - - // 头部 - if (head_enabled_) { - const auto& head = can_manager.head(); - for (const auto& motor : head.motors()) { - head_motors_cfg.push_back(motor); - } - head_can_client_ = std::make_shared(head); - head_can_sender_ = std::make_shared>(); - head_can_receiver_ = std::make_shared>(); - head_message_manager_ = std::make_shared>(); - } - - // 初始化 limbs - struct Limb { - std::string name; - bool enabled; - std::shared_ptr client; - std::shared_ptr> sender; - std::shared_ptr> receiver; - std::shared_ptr> message_manager; - std::vector motor_cfgs; - }; - - std::vector limbs{ - {"WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_, waist_motors_cfg}, - {"LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_, l_motors_cfg}, - {"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_, r_motors_cfg}, - {"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_, head_motors_cfg} - }; - - if (!motor_manager_) - { - motor_manager_ = std::make_shared(); - } - std::vector> 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 protocol = nullptr; - if (limb.sender && limb.message_manager) { - protocol = std::make_shared(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(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(); - } - auto protocol = std::make_shared(config.master().interface()); - for (const auto& motor_cfg : config.motors()) { - auto motor = std::make_shared(motor_cfg); - if (protocol) motor->setProtocol(protocol); - motor->init(); - motor_manager_->addMotor(motor); - } -} - bool MotorsInfo::getJointsQ(const std::vector &joints_name, std::unordered_map &joint_qs) const { auto res{true}; diff --git a/cmvr-es/device_manager/CMakeLists.txt b/cmvr-es/device_manager/CMakeLists.txt index d74d86c3..0b97b456 100644 --- a/cmvr-es/device_manager/CMakeLists.txt +++ b/cmvr-es/device_manager/CMakeLists.txt @@ -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 diff --git a/cmvr-es/device_manager/include/device_factory.h b/cmvr-es/device_manager/include/device_factory.h index b46e3495..1fcce739 100644 --- a/cmvr-es/device_manager/include/device_factory.h +++ b/cmvr-es/device_manager/include/device_factory.h @@ -28,7 +28,7 @@ namespace cmvr::device { template std::shared_ptr createFromConfig(const ConfigType& cfg); private: - std::shared_ptr create_agv_(const XmlNode& cfg); + std::shared_ptr create_agv_(const XmlNode& cfg); std::shared_ptr create_battery_(const XmlNode& cfg); std::shared_ptr create_camera_(const XmlNode& cfg); std::shared_ptr create_dexhand_(const XmlNode& cfg); diff --git a/cmvr-es/device_manager/include/device_manager.h b/cmvr-es/device_manager/include/device_manager.h index 8f54823b..ee782e5d 100644 --- a/cmvr-es/device_manager/include/device_manager.h +++ b/cmvr-es/device_manager/include/device_manager.h @@ -16,7 +16,7 @@ namespace cmvr::device { using DeviceVariant = std::variant< - std::shared_ptr, + std::shared_ptr, std::shared_ptr, std::shared_ptr, std::shared_ptr, diff --git a/cmvr-es/device_manager/src/device_factory.cpp b/cmvr-es/device_manager/src/device_factory.cpp index 84262f35..fba92e51 100644 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ b/cmvr-es/device_manager/src/device_factory.cpp @@ -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 DeviceFactory::create(const XmlNode&); +template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); @@ -33,7 +32,7 @@ template std::shared_ptr DeviceFactory::create template std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { - if constexpr (std::is_same_v) { + if constexpr (std::is_same_v) { return create_agv_(cfg); } else if constexpr (std::is_same_v) { return create_battery_(cfg); @@ -58,7 +57,7 @@ std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { } } -std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { +std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { return nullptr; } @@ -185,11 +184,6 @@ template std::shared_ptr DeviceFactory::createFromConfig DeviceFactory::createFromConfig(const config::PX6AXGen3& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegMicroPhoneConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegSpeakerConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::AGVsrc1100Config& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::BioHeadConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::HumanRobotConfig& cfg); -template std::shared_ptr DeviceFactory::createFromConfig(const config::EthercatRobotConfig& cfg); - template std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cfg) { @@ -250,50 +244,6 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); } } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; - throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); - } - } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - auto motos_info = MotorsInfo::getInstance(); - motos_info->init(cfg); - return std::make_shared>(cfg); - } - else if constexpr (std::is_same_v) - { - auto motos_info = MotorsInfo::getInstance(); - motos_info->init(cfg); - return std::make_shared>(cfg); - } - else - { - LOG(ERROR) << "[DeviceFactory]: Unsupported microphone device type "; - throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); - } - } - else if constexpr (std::is_same_v) - { - if constexpr (std::is_same_v) - { - return std::make_shared(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"); diff --git a/cmvr-es/device_manager/src/device_manager.cpp b/cmvr-es/device_manager/src/device_manager.cpp index 60a8c824..805516f9 100644 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ b/cmvr-es/device_manager/src/device_manager.cpp @@ -9,7 +9,7 @@ using namespace std; using namespace cmvr::device; using namespace cmvr::device; -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -152,7 +152,7 @@ void DeviceManager::getDeviceList(std::list> string dev_type; std::visit([&](const auto& ptr) { using T = std::decay_t; - if constexpr (std::is_same_v>) { + if constexpr (std::is_same_v>) { dev_type = "AGV"; } else if constexpr (std::is_same_v>) { 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(node); + auto device = dev_factory_->create(node); if (device == nullptr) { throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); } devices_[id] = device; - std::get>(devices_[id])->init(); + std::get>(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(robot); - devices_[id] = device; - std::get>(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>(devices_[id])->init(); } - config::HeadConfig head_cfg; - ConfigHelper::getHeadConfig(head_cfg); - for (int i = 0; i < head_cfg.bioheads().size(); i++) { - auto head = head_cfg.bioheads(i); - if (!head.enable()) - continue; - auto id = head.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Biohead Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Biohead Device ID" + id); + + auto biohead_node = dmgr_node.getChild("BioHead"); + for (auto &node: biohead_node.getChildren()){ + string id = node.getAttrString("id"); + if (devices_.count(id)){ + LOG(ERROR) << "[DeviceManager]: Duplicate AbstractBiohead Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate AbstractBiohead Device ID" + id); + } + auto device = dev_factory_->create(node); + if (device == nullptr){ + throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); } - std::cout << "Biohead[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(head); devices_[id] = device; std::get>(devices_[id])->init(); - } - 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(agv); - devices_[id] = device; - std::get>(devices_[id])->init(); } } catch (const exception& e) { diff --git a/cmvr-es/devices/CMakeLists.txt b/cmvr-es/devices/CMakeLists.txt index 21191941..41c64db3 100644 --- a/cmvr-es/devices/CMakeLists.txt +++ b/cmvr-es/devices/CMakeLists.txt @@ -6,4 +6,3 @@ add_subdirectory(biohead) add_subdirectory(robot) add_subdirectory(canbus) add_subdirectory(motor) -add_subdirectory(agv) diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt deleted file mode 100644 index 0d8b4bb9..00000000 --- a/cmvr-es/devices/agv/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -add_subdirectory(agv_src1100) - diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 858e3973..a838fa89 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -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 -#include -#include -#include -#include -#include -#include -#include +#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 maps; ///< 所有地图名称列表 - std::vector 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 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 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 target_point; ///< 目标坐标 [x, y, r]（若 task_type 为 1） - std::vector finished_path; ///< 已经过的站点列表 - std::vector unfinished_path; ///< 尚未经过的站点列表 - std::string move_status_info; ///< 导航附加信息（如错误描述） - std::vector 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 task_status_list; ///< 所有任务状态列表 - }; - - /** - * @brief 1110 批量查询任务入参 - */ - struct QueryTaskStatusPackageReq - { - std::vector 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 DI; ///< 需要等待的 DI 条件列表 - double timeout = 0.0; ///< 超时时间（秒），0 表示无限等待 - }; - /** - * @brief SetDO 动作参数 - */ - struct SetDOArgs - { - std::vector 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 str_kv; ///< 字符串键值对 - std::map num_kv; ///< 数字键值对 - std::vector do_list; ///< DO 列表（用于 setDO 等） - std::vector 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 \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_AGV_H \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt b/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt deleted file mode 100644 index f18e7907..00000000 --- a/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt +++ /dev/null @@ -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) \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h b/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h deleted file mode 100644 index 13ab8d32..00000000 --- a/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h +++ /dev/null @@ -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 -#include -#include -#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::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 connect_threads_; ///< 各端口异步连接线程 - mutable std::mutex connect_mutex_; ///< 保护 connect_threads_ 的互斥锁 -}; - -} // namespace cmvr::device - -#endif // AGV_SRC1100_H \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp b/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp deleted file mode 100644 index 558e621b..00000000 --- a/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp +++ /dev/null @@ -1,2771 +0,0 @@ -// ============================================================================ -// 文件名: agv_src1100.cpp -// 描述: 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现 -// 端口分类: -// 19204 - 机器人状态 API（查询位置/电池/任务/地图/控制权等） -// 19205 - 机器人控制 API（开环运动/切换地图等） -// 19206 - 机器人导航 API（路径导航/指定路径导航等） -// 19207 - 机器人配置 API（地图上传下载/控制权抢占等） -// 19210 - 其他 API（外设控制/音频/IO 等）【未实现】 -// 19301 - 机器人推送 API（主动状态推送）【未实现】 -// ============================================================================ - -#include "../include/agv_src1100.h" -#include -#include -#include -#include -#include -#include -#include -#include -// #include "json/json.h" -#include "nlohmann/json.hpp" -using json = nlohmann::json; -namespace cmvr::device -{ - // ============================================================================ - // 构造函数 / 析构函数 - // ============================================================================ - - /** - * @brief 从 Protobuf 配置对象构造设备实例 - * @param cfg AGVsrc1100Config 配置对象 - * @note 连接所有端口：19204/19205/19206/19207/19210/19301 - */ - AgvSrc1100::AgvSrc1100(const config::AGVsrc1100Config& cfg) : AbstractAgv() { - // 打印构造日志，便于调试追踪 - LOG(INFO) << "[AgvSrc1100 Create] this_addr=" << this - << " cfg_id=" << cfg.id() - << " cfg_ip=" << cfg.ip() - << " cfg_port_config=" << cfg.port_config(); - - // 从配置中读取设备标识信息 - id_ = cfg.id(); // 设备 ID - ip_ = cfg.ip(); // 设备 IP 地址 - enable_ = cfg.enable(); // 是否启用 - - // 初始化各端口号（对应仙工协议的不同服务端口） - port_status_ = cfg.port_status(); // 19204 – 状态查询 - port_control_ = cfg.port_control(); // 19205 – 控制指令 - port_nav_ = cfg.port_nav(); // 19206 – 导航任务 - port_config_ = cfg.port_config(); // 19207 – 配置操作 - port_other_ = cfg.port_other(); // 19210 – 其他外设 - port_push_ = cfg.port_push(); // 19301 – 数据推送 - - // 如果设备启用，则异步连接所有端口 - if (enable_) { - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_status_), port_status_); - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_control_), port_control_); - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_nav_), port_nav_); - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_config_), port_config_); - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_other_), port_other_); - connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_push_), port_push_); - } - - LOG(INFO) << "port_status=" << port_status_ - << ", port_control=" << port_control_ - << ", port_nav=" << port_nav_ - << ", port_config=" << port_config_ - << ", port_other=" << port_other_ - << ", port_push=" << port_push_; - } - - /** - * @brief 析构函数：停止设备并等待所有连接线程结束 - */ - AgvSrc1100::~AgvSrc1100() { - stop(); // 关闭所有 socket - // 等待所有异步连接线程结束 - for (auto& t : connect_threads_) { - if (t.joinable()) t.join(); - } - } - - // ============================================================================ - // 生命周期管理（通用接口） - // ============================================================================ - - /** - * @brief 获取当前设备运行状态 - * @return Status 枚举值 - */ - AbstractAgv::Status AgvSrc1100::state() const { - return status_; - } - - /** - * @brief 获取最后一次发生的错误信息 - * @return 错误描述字符串 - */ - std::string AgvSrc1100::lastError() const { - std::lock_guard lock(mutex_); - return last_error_; - } - - /** - * @brief 启动设备：标记为运行状态 - * @note socket 连接已在构造时完成，此处仅改变状态标志 - */ - void AgvSrc1100::start() { - status_ = Status::RUNNING; - } - - /** - * @brief 停止设备：关闭所有 TCP 连接并释放资源 - */ - void AgvSrc1100::stop() { - // 定义 lambda 关闭单个 socket 并置为 -1 - auto closeSocket = [](int& s) { - if (s >= 0) { close(s); s = -1; } - }; - // 关闭所有端口的 socket - closeSocket(sock_status_); - closeSocket(sock_control_); - closeSocket(sock_nav_); - closeSocket(sock_config_); - closeSocket(sock_other_); - closeSocket(sock_push_); - status_ = Status::STOPPED; - } - - /** - * @brief 周期性更新设备状态 - * @note 当前为空实现，预留扩展（如心跳检测、自动重连等） - */ - void AgvSrc1100::update() {} - - // ============================================================================ - // 私有通信辅助函数 - // ============================================================================ - - /** - * @brief 清空 socket 接收缓冲区残留数据，防止粘包 - * @param sock 目标 socket - * @note 非阻塞读取并丢弃所有可读数据 - */ - void AgvSrc1100::flushSocket(int sock) { - uint8_t tmp[128]; // 临时缓冲区 - ssize_t rlen; - // MSG_DONTWAIT: 非阻塞模式，读到没有数据时立即返回 - while ((rlen = recv(sock, tmp, sizeof(tmp), MSG_DONTWAIT)) > 0); - } - - /** - * @brief 设置 socket 接收超时时间 - * @param sock 目标 socket - * @param timeout_ms 超时时间（毫秒），0 表示取消超时 - */ - void AgvSrc1100::setSocketTimeout(int sock, int timeout_ms) { - struct timeval tv; - tv.tv_sec = timeout_ms / 1000; // 秒 - tv.tv_usec = (timeout_ms % 1000) * 1000; // 微秒 - // SO_RCVTIMEO: 设置接收超时 - setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); - } - - /** - * @brief 异步连接指定端口 - * @param sock 输出参数，连接成功后的 socket 描述符 - * @param port 目标端口号 - * @note 在独立线程中执行，连接失败时设置错误状态 - */ - void AgvSrc1100::asyncConnect(int& sock, int port) { - // 创建 TCP socket - int s = socket(AF_INET, SOCK_STREAM, 0); - if (s < 0) { - std::lock_guard lock(mutex_); - last_error_ = "socket create failed"; - status_ = Status::FAULT; - return; - } - - // 配置目标地址 - struct sockaddr_in addr{}; - addr.sin_family = AF_INET; - addr.sin_port = htons(port); - inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr); // IP 地址转换 - - // 发起 TCP 连接 - if (connect(s, (struct sockaddr*)&addr, sizeof(addr)) < 0) { - close(s); // 连接失败，关闭 socket - std::lock_guard lock(mutex_); - last_error_ = "connect failed port:" + std::to_string(port); - status_ = Status::FAULT; - return; - } - - // 连接成功，保存 socket 描述符 - { - std::lock_guard lock(mutex_); - sock = s; - status_ = Status::INITIALIZED; - } - } - - /** - * @brief 发送请求并接收完整响应 - * @param sock 已连接的 socket - * @param header 16 字节协议帧头（包含命令码等） - * @return true 表示成功，current_json_ 存储响应 JSON - * @note 适用于无负载或简单查询命令（如 1000/1004/1007） - */ - bool AgvSrc1100::sendAndRecv(int sock, const uint8_t* header) { - // 加锁，防止多个线程同时操作 current_json_ - std::lock_guard lock(mutex_); - - if (sock < 0) { - last_error_ = "socket not connected"; - return false; - } - - // 清空残留数据，防止读取到上一次的响应 - flushSocket(sock); - - // 发送 16 字节协议帧头 - if (send(sock, header, 16, MSG_NOSIGNAL) != 16) { - last_error_ = "send header failed"; - return false; - } - - // 设置接收超时 100ms，避免无限阻塞 - setSocketTimeout(sock, 100); - - const int RECV_BUF_LEN = 16384; - uint8_t recvBuf[RECV_BUF_LEN] = {0}; - int totalRecv = 0; - - // 循环接收数据，直到读空或缓冲区满 - while (totalRecv < RECV_BUF_LEN) - { - int ret = recv(sock, recvBuf + totalRecv, RECV_BUF_LEN - totalRecv, 0); - if (ret <= 0) break; // 读空或超时，退出循环 - totalRecv += ret; - } - - // 取消超时设置 - setSocketTimeout(sock, 0); - - if (totalRecv <= 0) { - last_error_ = "no data received"; - return false; - } - - // 将接收到的数据转为字符串 - std::string rawData(reinterpret_cast(recvBuf), totalRecv); - // 查找 JSON 起始和结束位置 - size_t jsonStart = rawData.find('{'); - size_t jsonEnd = rawData.rfind('}'); - - // 提取 JSON 内容，兜底处理不完整数据 - if (jsonStart == std::string::npos) - { - last_error_ = "can not find '{'"; - return false; - } - if (jsonEnd == std::string::npos) - { - // 找不到 }，截取到末尾并补上 } - current_json_ = rawData.substr(jsonStart) + "}"; - } - else - { - current_json_ = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); - } - - fflush(stdout); - last_error_ = "ok"; - return true; - } - - /** - * @brief 解析 JSON 字符串并做异常捕获 - * @param json_str 输入的 JSON 字符串 - * @param root 输出参数，解析后的 json 对象 - * @param err_msg 输出参数，解析失败时的错误信息 - * @return true 表示解析成功，false 表示失败 - */ - bool AgvSrc1100::parseJson(const std::string& json_str, json& root, std::string& err_msg) - { - root.clear(); - try - { - root = json::parse(json_str); // 使用 nlohmann/json 解析 - } - catch (json::parse_error& e) // 捕获解析异常 - { - err_msg = "JSON parse error: " + std::string(e.what()); - return false; - } - return true; - } - - // ============================================================================ - // 端口 19204 – 机器人状态 API（允许 10 个连接） - // 功能：查询机器人各种状态信息（只读操作） - // ============================================================================ - - // -------------------------------------------------------------------------- - // 1000 - robot_status_info_req 查询机器人基本信息 - // -------------------------------------------------------------------------- - void AgvSrc1100::getStatusInfo(AgvStatusInfo& info) { - int sock = sock_status_; - if (sock < 0) { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 构造 16 字节协议帧头 - // 格式: 0x5A 0x01 0x00 0x01 + 长度(4字节) + 命令码(2字节) + 保留(6字节) - // 命令码 0x03E8 = 1000 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x00, - 0x03, 0xE8, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - // 发送请求并接收响应 - if (!sendAndRecv(sock, send_buf)) { - info.ret_code = -2; - info.err_msg = last_error_; - return; - } - - // 从成员变量中读取响应 JSON - std::string json_str; - { - std::lock_guard lk(mutex_); - json_str = current_json_; - } - - // 兜底修复 JSON 格式（补全花括号） - if (!json_str.empty()) - { - if (json_str.front() != '{') json_str = "{" + json_str; - if (json_str.back() != '}') json_str += "}"; - } - - if (json_str.empty()) { - info.ret_code = -4; - info.err_msg = "empty json data"; - return; - } - - // 解析 JSON - json root; - std::string err; - if (!parseJson(json_str, root, err)) { - info.ret_code = -3; - info.err_msg = err; - return; - } - - // 提取字段，不存在的字段使用默认值 - info.id = root.value("id", ""); - info.vehicle_id = root.value("vehicle_id", ""); - info.version = root.value("version", ""); - info.model = root.value("model", ""); - info.dsp_version = root.value("dsp_version", ""); - info.current_ip = root.value("current_ip", ""); - info.mac = root.value("MAC", ""); - info.rssi = root.value("rssi", 0); - info.ret_code = root.value("ret_code", 0); - info.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 1007 - robot_status_battery_req 查询电池状态 - // -------------------------------------------------------------------------- - void AgvSrc1100::getBatteryStatus(BatteryStatus& info, bool simple) - { - int sock = sock_status_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 命令码 0x03EF = 1007 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x00, - 0x03, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - if (!sendAndRecv(sock, send_buf)) - { - info.ret_code = -2; - info.err_msg = last_error_; - return; - } - - std::string json_str; - { - std::lock_guard lk(mutex_); - json_str = current_json_; - } - - if (!json_str.empty()) - { - if (json_str.front() != '{') json_str = "{" + json_str; - if (json_str.back() != '}') json_str += "}"; - } - - if (json_str.empty()) - { - info.ret_code = -4; - info.err_msg = "empty json data"; - return; - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = err; - return; - } - - // 提取电池相关字段 - info.battery_level = root.value("battery_level", 0.0); - info.battery_temp = root.value("battery_temp", 0.0); - info.charging = root.value("charging", false); - info.voltage = root.value("voltage", 0.0); - info.current = root.value("current", 0.0); - info.max_charge_voltage = root.value("max_charge_voltage", -1.0); - info.max_charge_current = root.value("max_charge_current", -1.0); - info.manual_charge = root.value("manual_charge", false); - info.auto_charge = root.value("auto_charge", false); - info.battery_cycle = root.value("battery_cycle", 0); - info.battery_user_data = root.value("battery_user_data", ""); - info.extra = root.value("extra", ""); - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 1004 - robot_status_loc_req 查询机器人位置 - // -------------------------------------------------------------------------- - void AgvSrc1100::getRobotLocation(RobotLocation& info) - { - int sock = sock_status_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 命令码 0x03EC = 1004 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x00, - 0x03, 0xEC, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - if (!sendAndRecv(sock, send_buf)) - { - info.ret_code = -2; - info.err_msg = last_error_; - return; - } - - std::string json_str; - { - std::lock_guard lk(mutex_); - json_str = current_json_; - } - - if (!json_str.empty()) - { - if (json_str.front() != '{') json_str = "{" + json_str; - if (json_str.back() != '}') json_str += "}"; - } - - if (json_str.empty()) - { - info.ret_code = -4; - info.err_msg = "empty json data"; - return; - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = err; - return; - } - - // 提取位置相关字段 - info.x = root.value("x", 0.0); - info.y = root.value("y", 0.0); - info.angle = root.value("angle", 0.0); - info.confidence = root.value("confidence", 0.0); - info.current_station = root.value("current_station", ""); - info.last_station = root.value("last_station", ""); - info.loc_method = root.value("loc_method", 0); - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 1300 - robot_status_map_req 查询地图状态 - // -------------------------------------------------------------------------- - void AgvSrc1100::getMapStatus(MapStatus& info) - { - int sock = sock_status_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 命令码 0x0514 = 1300 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x00, - 0x05, 0x14, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - if (!sendAndRecv(sock, send_buf)) - { - info.ret_code = -2; - info.err_msg = last_error_; - return; - } - - std::string json_str; - { - std::lock_guard lk(mutex_); - json_str = current_json_; - } - - if (!json_str.empty()) - { - if (json_str.front() != '{') json_str = "{" + json_str; - if (json_str.back() != '}') json_str += "}"; - } - - if (json_str.empty()) - { - info.ret_code = -4; - info.err_msg = "empty json data"; - return; - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = err; - return; - } - - // 提取地图字段 - info.current_map = root.value("current_map", ""); - info.current_map_md5 = root.value("current_map_md5", ""); - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - - // 解析地图名称数组 - if (root.contains("maps") && root["maps"].is_array()) - { - for (auto& item : root["maps"]) - { - info.maps.push_back(item.get()); - } - } - - // 解析地图文件详情数组 - if (root.contains("map_files_info") && root["map_files_info"].is_array()) - { - for (auto& item : root["map_files_info"]) - { - MapFileInfo fileInfo; - fileInfo.name = item.value("name", ""); - fileInfo.modified = item.value("modified", ""); - fileInfo.size = item.value("size", 0LL); - info.map_files_info.push_back(fileInfo); - } - } - } - - - // -------------------------------------------------------------------------- - // 1022 - robot_status_loadmap_req 查询地图载入状态 - // -------------------------------------------------------------------------- - void AgvSrc1100::queryLoadMapStatus(QueryLoadMapStatusRes& res) - { - int sock = sock_status_; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "socket not connected"; - return; - } - - // 命令码 0x03FE = 1022，无负载，长度=0 - uint16_t payload_len = 0; - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), // 长度高字节 - static_cast(payload_len & 0xFF), // 长度低字节 - 0x03, 0xFE, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); // 1秒超时 - - // 发送帧头（有重试机制） - int total_sent = 0; - int retry_cnt = 3; - while (total_sent < 16 && retry_cnt > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) - { - retry_cnt--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 接收响应 - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取 JSON - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - // 解析 JSON - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) - { - res.ret_code = -3; - res.err_msg = "json parse error: " + parse_err; - return; - } - - // loadmap_status: 0=失败, 1=成功, 2=载入中 - res.loadmap_status = root.value("loadmap_status", 0); - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 1301 - robot_status_station_req 查询站点列表 - // -------------------------------------------------------------------------- - void AgvSrc1100::queryStationList(QueryStationRes& res) - { - int sock = sock_status_; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "socket not connected"; - return; - } - - // 命令码 0x0515 = 1301，无负载，长度=0 - uint16_t payload_len = 0; - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x05, 0x15, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - int total_sent = 0; - int retry_cnt = 3; - while (total_sent < 16 && retry_cnt > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) - { - retry_cnt--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) - { - res.ret_code = -3; - res.err_msg = "json parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - - // 解析站点数组 - if (root.contains("stations") && root["stations"].is_array()) - { - for (auto& item : root["stations"]) - { - StationItem st; - st.id = item.value("id", ""); - st.type = item.value("type", ""); - st.x = item.value("x", 0.0); - st.y = item.value("y", 0.0); - st.r = item.value("r", 0.0); - st.desc = item.value("desc", ""); - st.executor = item.value("executor", ""); - st.prepoint = item.value("prepoint", ""); - st.recfile = item.value("recfile", ""); - - // 兼容 bool 和字符串 "true"/"false" - if (item.contains("spin")) - { - if (item["spin"].is_boolean()) - st.spin = item["spin"].get(); - else if (item["spin"].is_string()) - st.spin = (item["spin"].get() == "true"); - } - if (item.contains("use_down_pgv")) - { - if (item["use_down_pgv"].is_boolean()) - st.use_down_pgv = item["use_down_pgv"].get(); - else if (item["use_down_pgv"].is_string()) - st.use_down_pgv = (item["use_down_pgv"].get() == "true"); - } - - res.stations.push_back(st); - } - } - } - - - // -------------------------------------------------------------------------- - // 1060 - robot_status_current_lock_req 查询当前控制权所有者 - // -------------------------------------------------------------------------- - void AgvSrc1100::getCurrentLockStatus(CurrentLockStatus& info) - { - int sock = sock_status_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 命令码 0x0424 = 1060，无负载，长度=0 - uint16_t payload_len = 0; - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x04, 0x24, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, 0); - if (ret <= 0) - { - retry--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; - } - - const int RECV_BUF_LEN = 65536; - std::vector recv_buf(RECV_BUF_LEN); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), RECV_BUF_LEN, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - info.ret_code = -2; - info.err_msg = "no response data"; - return; - } - - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - info.ret_code = -4; - info.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = "json parse error: " + err; - return; - } - - // 提取控制权信息 - info.locked = root.value("locked", false); - info.ip = root.value("ip", ""); - info.port = root.value("port", 0); - info.type = static_cast(root.value("type", 0)); - info.nick_name = root.value("nick_name", ""); - info.time_t = root.value("time_t", 0LL); - info.desc = root.value("desc", ""); - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 1020 - robot_status_task_req 查询当前导航状态 - // -------------------------------------------------------------------------- - void AgvSrc1100::robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& req) - { - int sock = sock_status_; - LOG(INFO) << "robotStatusTaskCurrent sock fd=" << sock; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "status port socket invalid"; - return; - } - - // 组装请求 JSON: {"simple": true/false} - json send_json; - send_json["simple"] = req.simple; - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - - // 命令码 0x03FC = 1020 - static uint8_t frame_header[16] = { - 0x5A,0x01,0x00,0x01, - 0x00,0x00, - 0,0, - 0x03,0xFC, - 0,0,0,0,0,0 - }; - frame_header[6] = static_cast((payload_len >> 8) & 0xFF); - frame_header[7] = static_cast(payload_len & 0xFF); - - flushSocket(sock); - setSocketTimeout(sock, 3000); - - // 发送帧头 - int sent = 0; - int retry = 5; - while (sent < 16 && retry > 0) - { - int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); - if (w <= 0) - { - retry--; - usleep(20000); - continue; - } - sent += w; - } - if (sent != 16) - { - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload - const int slice = 1024; - size_t off = 0; - size_t total = payload.size(); - const char* data = payload.data(); - while (off < total) - { - int wlen = static_cast(std::min((size_t)slice, total - off)); - int w = send(sock, data + off, wlen, MSG_NOSIGNAL); - if (w <= 0) - { - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - off += w; - } - - // 接收响应 - std::string raw; - uint8_t buf[65536]; - int rlen = 0; - while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) - { - raw.append((char*)buf, rlen); - } - if (raw.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取 JSON - size_t json_begin = raw.find('{'); - if (json_begin == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "response no json"; - return; - } - std::string json_str = raw.substr(json_begin); - if (json_str.back() != '}') - json_str += "}"; - - json root; - try - { - root = json::parse(json_str); - } - catch (json::parse_error& e) - { - res.ret_code = -3; - res.err_msg = std::string("json parse err:") + e.what(); - return; - } - - // 提取公共字段 - res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; - res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; - res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; - - // 提取导航状态字段 - // task_status: 0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED - res.task_status = root.contains("task_status") ? root["task_status"].get() : 0; - // task_type: 0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点 - res.task_type = root.contains("task_type") ? root["task_type"].get() : 0; - res.target_id = root.contains("target_id") ? root["target_id"].get() : ""; - res.move_status_info = root.contains("move_status_info") ? root["move_status_info"].get() : ""; - - // 提取目标坐标 [x, y, r] - if (root.contains("target_point") && root["target_point"].is_array()) - { - for (auto& val : root["target_point"]) - res.target_point.push_back(val.get()); - } - - // 提取已经过的站点列表 - if (root.contains("finished_path") && root["finished_path"].is_array()) - { - for (auto& s : root["finished_path"]) - res.finished_path.push_back(s.get()); - } - - // 提取尚未经过的站点列表 - if (root.contains("unfinished_path") && root["unfinished_path"].is_array()) - { - for (auto& s : root["unfinished_path"]) - res.unfinished_path.push_back(s.get()); - } - - // 提取料箱信息 - if (root.contains("containers") && root["containers"].is_array()) - { - for (auto& item : root["containers"]) - { - NavContainerItem c; - c.container_name = item.contains("container_name") ? item["container_name"].get() : ""; - c.desc = item.contains("desc") ? item["desc"].get() : ""; - c.goods_id = item.contains("goods_id") ? item["goods_id"].get() : ""; - c.has_goods = item.contains("has_goods") ? item["has_goods"].get() : false; - res.containers.push_back(c); - } - } - } - - - // -------------------------------------------------------------------------- - // 1110 - robot_status_task_status_package_req 批量查询任务状态 - // -------------------------------------------------------------------------- - void AgvSrc1100::robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) - { - int sock = sock_status_; - LOG(INFO) << "robotStatusTaskPackage sock fd=" << sock; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "status port socket invalid"; - return; - } - - // 强制携带 task_ids 字段，空数组也生成，避免设备报 60002 错误 - json send_json; - json arr = json::array(); - for (auto& tid : req.task_ids) - arr.push_back(tid); - send_json["task_ids"] = arr; - - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - - // 命令码 0x0456 = 1110 - static uint8_t frame_header[16] = { - 0x5A,0x01,0x00,0x01, - 0x00,0x00, - 0,0, - 0x04,0x56, - 0,0,0,0,0,0 - }; - frame_header[6] = static_cast((payload_len >> 8) & 0xFF); - frame_header[7] = static_cast(payload_len & 0xFF); - - flushSocket(sock); - setSocketTimeout(sock, 3000); - - // 发送帧头 - int sent = 0; - int retry = 5; - while (sent < 16 && retry > 0) - { - int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); - if (w <= 0) - { - retry--; - usleep(20000); - continue; - } - sent += w; - } - if (sent != 16) - { - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload - const int slice = 1024; - size_t off = 0; - size_t total = payload.size(); - const char* data = payload.data(); - while (off < total) - { - int wlen = static_cast(std::min((size_t)slice, total - off)); - int w = send(sock, data + off, wlen, MSG_NOSIGNAL); - if (w <= 0) - { - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - off += w; - } - - // 接收响应 - std::string raw; - uint8_t buf[65536]; - int rlen = 0; - while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) - { - raw.append((char*)buf, rlen); - } - if (raw.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取 JSON - size_t json_begin = raw.find('{'); - if (json_begin == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "response no json"; - return; - } - std::string json_str = raw.substr(json_begin); - if (json_str.back() != '}') - json_str += "}"; - - json root; - try - { - root = json::parse(json_str); - } - catch (json::parse_error& e) - { - res.ret_code = -3; - res.err_msg = std::string("json parse err:") + e.what(); - return; - } - - // 提取公共字段 - res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; - res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; - res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; - - // 解析 task_status_package 对象 - if (root.contains("task_status_package")) - { - auto pkg_json = root["task_status_package"]; - auto& pkg_out = res.task_status_package; - - pkg_out.closest_target = pkg_json.contains("closest_target") ? pkg_json["closest_target"].get() : ""; - pkg_out.source_name = pkg_json.contains("source_name") ? pkg_json["source_name"].get() : ""; - pkg_out.target_name = pkg_json.contains("target_name") ? pkg_json["target_name"].get() : ""; - pkg_out.percentage = pkg_json.contains("percentage") ? pkg_json["percentage"].get() : 0.0; - pkg_out.distance = pkg_json.contains("distance") ? pkg_json["distance"].get() : 0.0; - pkg_out.info = pkg_json.contains("info") ? pkg_json["info"].get() : ""; - - // 解析任务状态列表 - if (pkg_json.contains("task_status_list") && pkg_json["task_status_list"].is_array()) - { - for (auto& item : pkg_json["task_status_list"]) - { - SingleTaskStatusItem st; - st.task_id = item.contains("task_id") ? item["task_id"].get() : ""; - st.status = item.contains("status") ? item["status"].get() : 0; - st.type = item.contains("type") ? item["type"].get() : 0; - pkg_out.task_status_list.push_back(st); - } - } - } - } - - - // ============================================================================ - // 端口 19205 – 机器人控制 API（允许 5 个连接） - // 功能：下发控制指令，改变机器人运动或状态 - // ============================================================================ - - // -------------------------------------------------------------------------- - // 2010 - robot_control_motion_req 开环速度运动控制 - // -------------------------------------------------------------------------- - void AgvSrc1100::robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) - { - int sock = sock_control_; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "socket not connected"; - return; - } - - // 组装请求 JSON，只包含非零有效字段 - json send_json; - if (req.vx != 0.0) send_json["vx"] = req.vx; // X 方向速度 - if (req.vy != 0.0) send_json["vy"] = req.vy; // Y 方向速度 - if (req.w != 0.0) send_json["w"] = req.w; // 角速度 - if (req.steer != 0.0) send_json["steer"] = req.steer; // 舵角（单舵轮） - if (req.real_steer != 0.0) send_json["real_steer"] = req.real_steer; // 实际舵角 - if (req.duration >= 0) send_json["duration"] = req.duration; // 运动持续时间 - - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - - // 命令码 0x07DA = 2010 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x07, 0xDA, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - int total_sent = 0; - int retry_cnt = 3; - while (total_sent < 16 && retry_cnt > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) - { - retry_cnt--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload - const int slice_size = 4096; - size_t offset = 0; - size_t total_size = payload.size(); - const char* data_ptr = payload.data(); - while (offset < total_size) - { - int send_len = std::min((size_t)slice_size, total_size - offset); - int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); - if (ret <= 0) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - offset += ret; - usleep(500); - } - - // 接收响应 - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) - { - res.ret_code = -3; - res.err_msg = "json parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 2022 - robot_control_loadmap_req 切换载入地图 - // -------------------------------------------------------------------------- - void AgvSrc1100::robotLoadMap(LoadMapRes& res, const LoadMapReq& req) - { - int sock = sock_control_; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "socket not connected"; - return; - } - - // 组装请求 JSON: {"map_name":"xxx"} - json send_json; - send_json["map_name"] = req.map_name; - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - - // 命令码 0x07E6 = 2022 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x07, 0xE6, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - // 发送帧头 - int total_sent = 0; - int retry_cnt = 3; - while (total_sent < 16 && retry_cnt > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) - { - retry_cnt--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload - const int slice_size = 4096; - size_t offset = 0; - size_t total_size = payload.size(); - const char* data_ptr = payload.data(); - while (offset < total_size) - { - int send_len = std::min((size_t)slice_size, total_size - offset); - int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); - if (ret <= 0) - { - setSocketTimeout(sock, 3000); - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - offset += ret; - usleep(500); - } - - // 接收响应 - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) - { - res.ret_code = -3; - res.err_msg = "json parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - LOG(INFO) << "recv raw_data len=" << raw_data.size() << " content=" << raw_data; - } - - - // -------------------------------------------------------------------------- - // 2000 - robot_control_stop_req 停止开环运动 命令码0x07D0，应答码12000(0x2EE0) - // 下发零速度开环指令，终止当前持续运动 - // -------------------------------------------------------------------------- - // -------------------------------------------------------------------------- - // 2000 - robot_control_stop_req 停止开环运动 命令码0x07D0，应答码12000(0x2EE0) - // 下发零速度开环指令，终止当前持续运动 - // -------------------------------------------------------------------------- - - void AgvSrc1100::robotControlStop(RobotControlStopRes& res) - { - int sock = sock_control_; - if (sock < 0) { - res.ret_code = -1; - res.err_msg = "control socket disconnected"; - return; - } - - // 业务负载长度为 0 - uint32_t payload_len = 0; - - // 标准 16 字节帧头（长度 4 字节，命令码 0x07D0，保留 6 字节） - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - static_cast((payload_len >> 24) & 0xFF), - static_cast((payload_len >> 16) & 0xFF), - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x07, 0xD0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); // 适当增加超时 - - // 发送帧头（重试机制） - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry-- > 0) { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) { - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send header failed"; - return; - } - - // 无 payload，无需发送额外数据 - - // 接收响应（使用较大缓冲区） - const int RECV_BUF = 131072; - std::vector recv_buf(RECV_BUF); - std::string raw_data; - int rlen; - while ((rlen = recv(sock, recv_buf.data(), RECV_BUF, MSG_NOSIGNAL)) > 0) { - raw_data.append(reinterpret_cast(recv_buf.data()), rlen); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) { - res.ret_code = -2; - res.err_msg = "no response from device"; - return; - } - - // 提取 JSON（容错补全） - size_t json_start = raw_data.find('{'); - if (json_start == std::string::npos) { - res.ret_code = -4; - res.err_msg = "no JSON in response"; - return; - } - std::string json_str = raw_data.substr(json_start); - // 补全缺失的 '}' - int left = std::count(json_str.begin(), json_str.end(), '{'); - int right = std::count(json_str.begin(), json_str.end(), '}'); - while (right < left) { json_str += "}"; right++; } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) { - res.ret_code = -3; - res.err_msg = "JSON parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - } - - - // ============================================================================ - // 端口 19206 – 机器人导航 API（允许 5 个连接） - // 功能：下发导航任务 - // ============================================================================ - - // -------------------------------------------------------------------------- - // 3066 - robot_task_gotargetlist_req 指定路径导航（多点） - // 功能：下发多段连续站点导航任务序列，任务串行排队执行 - // 必填字段：task_id / source_id / id - // -------------------------------------------------------------------------- - void AgvSrc1100::robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) - { - int sock = sock_nav_; - LOG(INFO) << "robotGoTargetList sock fd=" << sock; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "nav socket invalid"; - return; - } - - // 组装请求 JSON: {"move_task_list": [{"task_id":"xxx","source_id":"xxx","id":"xxx"}, ...]} - json send_json; - json arr = json::array(); - for (const auto& task : req.move_task_list) - { - json obj; - obj["task_id"] = task.task_id; - obj["source_id"] = task.source_id; - obj["id"] = task.id; - if (!task.operation.empty()) - { - obj["operation"] = task.operation; - obj["jack_height"] = task.jack_height; - } - arr.push_back(obj); - } - send_json["move_task_list"] = arr; - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - LOG(INFO) << "nav payload length:" << payload_len; - - // 命令码 0x0BFA = 3066 - static uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - 0,0, - 0x0B, 0xFA, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - send_buf[6] = static_cast((payload_len >> 8) & 0xFF); - send_buf[7] = static_cast(payload_len & 0xFF); - - flushSocket(sock); - setSocketTimeout(sock, 3000); - - // 发送帧头，带重试 - int total_sent = 0; - int retry_cnt = 5; - while (total_sent < 16 && retry_cnt > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) - { - LOG(ERROR) << "send header failed, ret=" << ret - << ", errno=" << errno - << " (" << strerror(errno) << ")"; - retry_cnt--; - usleep(20000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload（1024 字节一片） - const int slice_size = 1024; - size_t offset = 0; - size_t total_size = payload.size(); - const char* data_ptr = payload.data(); - while (offset < total_size) - { - int send_len = std::min((size_t)slice_size, total_size - offset); - int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); - if (ret <= 0) - { - LOG(ERROR) << "send payload failed, errno=" << errno << " (" << strerror(errno) << ")"; - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - offset += ret; - } - - // 接收响应 - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, MSG_NOSIGNAL)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t json_start = raw_data.find('{'); - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "json not found in response"; - return; - } - std::string json_str = raw_data.substr(json_start); - if (json_str.back() != '}') - { - json_str += "}"; - } - - json root; - std::string parse_err; - try - { - root = json::parse(json_str); - } - catch (json::parse_error& e) - { - res.ret_code = -3; - res.err_msg = "json parse error: " + std::string(e.what()); - return; - } - - res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; - res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; - res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; - } - - - // -------------------------------------------------------------------------- - // 3051 - robot_task_gotarget_req 单点路径导航（自动规划） - // 功能：给定目标站点，机器人自动规划路径前往 - // 注意：仅用于单车测试，严禁用于多车调度场景 - // -------------------------------------------------------------------------- - void AgvSrc1100::robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) - { - int sock = sock_nav_; - LOG(INFO) << "robotGoTarget control sock fd=" << sock; - if (sock < 0) - { - res.ret_code = -1; - res.err_msg = "control port socket invalid"; - return; - } - - // 组装请求 JSON（根据协议 3051 的字段结构） - json send_json; - // 必填字段 - send_json["source_id"] = req.source_id; - send_json["id"] = req.id; - if (!req.task_id.empty()) - send_json["task_id"] = req.task_id; - - // 基础运动参数（只添加非默认值） - if (req.angle != 0) send_json["angle"] = req.angle; - if (!req.method.empty()) send_json["method"] = req.method; - if (req.max_speed != 0) send_json["max_speed"] = req.max_speed; - if (req.max_wspeed != 0) send_json["max_wspeed"] = req.max_wspeed; - if (req.max_acc != 0) send_json["max_acc"] = req.max_acc; - if (req.max_wacc != 0) send_json["max_wacc"] = req.max_wacc; - if (req.duration > 0) send_json["duration"] = req.duration; - if (req.orientation != 0) send_json["orientation"] = req.orientation; - if (req.spin) send_json["spin"] = true; - if (req.delay > 0) send_json["delay"] = req.delay; - if (req.start_rot_dir != 0) send_json["start_rot_dir"] = req.start_rot_dir; - if (req.end_rot_dir != 0) send_json["end_rot_dir"] = req.end_rot_dir; - if (req.reach_dist != 0) send_json["reach_dist"] = req.reach_dist; - if (req.reach_angle != 0) send_json["reach_angle"] = req.reach_angle; - if (!req.skill_name.empty()) send_json["skill_name"] = req.skill_name; - - // PGV 二次定位参数 - auto& pgv = req.pgv; - if (pgv.use_pgv) send_json["use_pgv"] = true; - if (pgv.use_down_pgv) send_json["use_down_pgv"] = true; - if (pgv.pgv_adjust_dist != 0) send_json["pgv_adjust_dist"] = pgv.pgv_adjust_dist; - if (pgv.pgv_adjust_cx != 0) send_json["pgv_adjust_cx"] = pgv.pgv_adjust_cx; - if (pgv.pgv_adjust_cy != 0) send_json["pgv_adjust_cy"] = pgv.pgv_adjust_cy; - if (pgv.pgv_x_adjust != 0) send_json["pgv_x_adjust"] = pgv.pgv_x_adjust; - - // 设备操作（顶升/货叉/辊筒/牵引/音频/等待DI/设置DO/脚本） - if (!req.operation.empty()) - { - send_json["operation"] = req.operation; - - // 顶升设备 JackLoad/JackUnload/JackHeight - if (req.operation.substr(0,4) == "Jack") - { - if (req.jack_height != 0) send_json["jack_height"] = req.jack_height; - } - // 货叉设备 ForkLoad/ForkUnload/ForkHeight/ForkForward - else if (req.operation.substr(0,4) == "Fork") - { - if (req.start_height != 0) send_json["start_height"] = req.start_height; - if (req.end_height != 0) send_json["end_height"] = req.end_height; - if (req.fork_mid_height != 0) send_json["fork_mid_height"] = req.fork_mid_height; - if (req.fork_dist != 0) send_json["fork_dist"] = req.fork_dist; - } - // 辊筒设备 RollerLoad/RollerUnload/RollerRoll/... - else if (req.operation.substr(0,6) == "Roller") - { - if (!req.direction.empty()) send_json["direction"] = req.direction; - } - // 牵引设备 HookLoad/HookUnload - else if (req.operation.substr(0,4) == "Hook") - { - if (req.recognize) send_json["recognize"] = true; - } - // 音频设备 sound - else if (req.operation == "sound") - { - json sound; - auto& sa = req.sounds_args; - if (!sa.name.empty()) sound["name"] = sa.name; - if (sa.loop != 0) sound["loop"] = sa.loop; - if (sa.stop != 0) sound["stop"] = sa.stop; - send_json["sounds_args"] = sound; - } - // 等待 DI 触发 - else if (req.operation == "WaitDI") - { - json args_obj; - json di_arr = json::array(); - for (auto& di : req.wait_di_args.DI) - { - json di_item; - di_item["id"] = di.id; - di_item["status"] = di.status; - di_arr.push_back(di_item); - } - args_obj["DI"] = di_arr; - if (req.wait_di_args.timeout > 0) args_obj["timeout"] = req.wait_di_args.timeout; - send_json["args"] = args_obj; - } - // 控制 DO 开关 - else if (req.operation == "SetDO") - { - json args_obj; - json do_arr = json::array(); - for (auto& d : req.set_do_args.DO) - { - json do_item; - do_item["id"] = d.id; - do_item["status"] = d.status; - do_arr.push_back(do_item); - } - args_obj["DO"] = do_arr; - send_json["args"] = args_obj; - } - } - - // 视觉识别标志 - if (req.recognize) send_json["recognize"] = true; - if (!req.recfile.empty()) send_json["recfile"] = req.recfile; - - // 脚本执行 - if (!req.script_name.empty()) - { - send_json["script_name"] = req.script_name; - send_json["script_stage"] = req.script_stage; - json s_arg; - for (auto& kv : req.script_args.str_kv) s_arg[kv.first] = kv.second; - for (auto& kv : req.script_args.num_kv) s_arg[kv.first] = kv.second; - json do_arr = json::array(); - for (auto& d : req.script_args.do_list) - { - json o; o["id"] = d.id; o["status"] = d.status; do_arr.push_back(o); - } - if (!do_arr.empty()) s_arg["DO"] = do_arr; - json di_arr = json::array(); - for (auto& di : req.script_args.di_list) - { - json o; o["id"] = di.id; o["status"] = di.status; di_arr.push_back(o); - } - if (!di_arr.empty()) s_arg["DI"] = di_arr; - send_json["script_args"] = s_arg; - } - - // 原地旋转（GoByOdometer） - if (req.move_angle != 0) - { - send_json["move_angle"] = req.move_angle; - send_json["speed_w"] = req.speed_w; - send_json["loc_mode"] = req.loc_mode; - } - - // 自由导航（仅双轮差速底盘） - if (req.freeGo.x != 0 || req.freeGo.y != 0) - { - json fg; - fg["x"] = req.freeGo.x; - fg["y"] = req.freeGo.y; - fg["theta"] = req.freeGo.theta; - send_json["freeGo"] = fg; - } - - std::string payload = send_json.dump(); - uint16_t payload_len = static_cast(payload.size()); - - // 命令码 0x0BEB = 3051 - uint8_t frame_header[16] = { - 0x5A,0x01,0x00,0x01, - 0x00,0x00, - 0,0, - 0x0B,0xEB, - 0,0,0,0,0,0 - }; - frame_header[6] = static_cast((payload_len >> 8) & 0xFF); - frame_header[7] = static_cast(payload_len & 0xFF); - - flushSocket(sock); - setSocketTimeout(sock, 3000); - - // 发送帧头 - int sent = 0; - int retry = 5; - while (sent < 16 && retry > 0) - { - int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); - if (w <= 0) - { - retry--; - usleep(20000); - continue; - } - sent += w; - } - if (sent != 16) - { - res.ret_code = -2; - res.err_msg = "send frame header failed"; - return; - } - - // 分片发送 payload - const int slice = 1024; - size_t off = 0; - size_t total = payload.size(); - const char* data = payload.data(); - while (off < total) - { - int wlen = static_cast(std::min((size_t)slice, total - off)); - int w = send(sock, data + off, wlen, MSG_NOSIGNAL); - if (w <= 0) - { - res.ret_code = -2; - res.err_msg = "send payload failed"; - return; - } - off += w; - } - - // 接收响应 - std::string raw; - uint8_t buf[65536]; - int rlen = 0; - while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) - { - raw.append((char*)buf, rlen); - } - if (raw.empty()) - { - res.ret_code = -2; - res.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t json_start = raw.find('{'); - if (json_start == std::string::npos) - { - res.ret_code = -4; - res.err_msg = "response no valid json"; - return; - } - std::string json_str = raw.substr(json_start); - if (json_str.back() != '}') json_str += "}"; - - json root; - try - { - root = json::parse(json_str); - } - catch (json::parse_error& e) - { - res.ret_code = -3; - res.err_msg = std::string("json parse err:") + e.what(); - return; - } - - res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; - res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; - res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; - } - - - // ============================================================================ - // 端口 19207 – 机器人配置 API（允许 5 个连接） - // 功能：配置类操作（地图上传下载、控制权管理等） - // ============================================================================ - - // -------------------------------------------------------------------------- - // 4005 - robot_config_lock_req 抢占控制权 - // 功能：向机器人申请独占控制权，成功后获得操作权限 - // -------------------------------------------------------------------------- - void AgvSrc1100::lockRobotControl(LockResult& info, const std::string& nick_name) - { - int sock = sock_config_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 组装请求 JSON: {"nick_name":"xxx"} - std::string req_json = R"({"nick_name":")" + nick_name + R"("})"; - uint16_t payload_len = static_cast(req_json.size()); - - // 命令码 0x0FA5 = 4005 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x0F, 0xA5, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - // 发送帧头 - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry > 0) - { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, 0); - if (ret <= 0) - { - retry--; - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) - { - setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; - } - - // 分片发送 JSON 负载 - const int slice_size = 4096; - size_t offset = 0; - size_t total_size = req_json.size(); - const char* data = req_json.c_str(); - while (offset < total_size) - { - int send_len = std::min((size_t)slice_size, total_size - offset); - int ret = send(sock, data + offset, send_len, 0); - if (ret <= 0) - { - setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send payload failed"; - return; - } - offset += ret; - usleep(500); - } - - // 接收响应 - const int recv_buf_len = 65536; - std::vector recv_buf(recv_buf_len); - std::string raw_data; - int recv_ret = 0; - while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) - { - raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); - } - setSocketTimeout(sock, 0); - - if (raw_data.empty()) - { - info.ret_code = -2; - info.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t json_start = raw_data.find('{'); - size_t json_end = raw_data.rfind('}'); - std::string json_str; - if (json_start == std::string::npos) - { - info.ret_code = -4; - info.err_msg = "json not found in response"; - return; - } - if (json_end == std::string::npos) - { - json_str = raw_data.substr(json_start) + "}"; - } - else - { - json_str = raw_data.substr(json_start, json_end - json_start + 1); - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = "json parse error: " + err; - return; - } - - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - } - - - - // ============================================================================ - // 3003 - robot_task_cancel_req 取消当前导航任务 (导航端口 19206) - // ============================================================================ - void AgvSrc1100::robotTaskCancel(RobotTaskCancelRes& res) { - int sock = sock_nav_; - if (sock < 0) { - res.ret_code = -1; - res.err_msg = "nav socket disconnected"; - return; - } - - uint32_t payload_len = 0; - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - static_cast((payload_len >> 24) & 0xFF), - static_cast((payload_len >> 16) & 0xFF), - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x0B, 0xBB, // 命令码 3003 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry-- > 0) { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) { - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send header failed"; - return; - } - - // 接收响应（先读 16 字节头，再按长度读 JSON） - uint8_t recv_header[16]; - size_t header_read = 0; - while (header_read < 16) { - int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv header failed or timeout"; - return; - } - header_read += ret; - } - - uint32_t data_len = (recv_header[4] << 24) | - (recv_header[5] << 16) | - (recv_header[6] << 8) | - recv_header[7]; - - std::string json_str; - if (data_len > 0) { - json_str.resize(data_len); - size_t total_read = 0; - while (total_read < data_len) { - int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv payload failed or timeout"; - return; - } - total_read += ret; - } - } - setSocketTimeout(sock, 0); - - if (json_str.empty()) { - res.ret_code = -4; - res.err_msg = "empty json data"; - return; - } - - // 容错提取 JSON - size_t json_start = json_str.find('{'); - if (json_start == std::string::npos) { - res.ret_code = -4; - res.err_msg = "no JSON found in payload"; - return; - } - size_t json_end = json_str.rfind('}'); - if (json_end != std::string::npos) { - json_str = json_str.substr(json_start, json_end - json_start + 1); - } else { - json_str = json_str.substr(json_start) + "}"; - } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) { - res.ret_code = -3; - res.err_msg = "JSON parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - - LOG(INFO) << "robotTaskCancel success, ret_code=" << res.ret_code; - } - - // ============================================================================ - // 3002 - robot_task_resume_req 继续当前导航任务 (导航端口 19206) - // ============================================================================ - void AgvSrc1100::robotTaskResume(RobotTaskResumeRes& res) { - int sock = sock_nav_; - if (sock < 0) { - res.ret_code = -1; - res.err_msg = "nav socket disconnected"; - return; - } - - uint32_t payload_len = 0; - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - static_cast((payload_len >> 24) & 0xFF), - static_cast((payload_len >> 16) & 0xFF), - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x0B, 0xBA, // 命令码 3002 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); - - // 发送帧头（带重试） - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry-- > 0) { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) { - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send header failed"; - return; - } - - // 接收响应（先读 16 字节头，再按长度读 JSON） - uint8_t recv_header[16]; - size_t header_read = 0; - while (header_read < 16) { - int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv header failed or timeout"; - return; - } - header_read += ret; - } - - uint32_t data_len = (recv_header[4] << 24) | - (recv_header[5] << 16) | - (recv_header[6] << 8) | - recv_header[7]; - - std::string json_str; - if (data_len > 0) { - json_str.resize(data_len); - size_t total_read = 0; - while (total_read < data_len) { - int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv payload failed or timeout"; - return; - } - total_read += ret; - } - } - setSocketTimeout(sock, 0); - - if (json_str.empty()) { - res.ret_code = -4; - res.err_msg = "empty json data"; - return; - } - - // 容错提取 JSON - size_t json_start = json_str.find('{'); - if (json_start == std::string::npos) { - res.ret_code = -4; - res.err_msg = "no JSON found in payload"; - return; - } - size_t json_end = json_str.rfind('}'); - if (json_end != std::string::npos) { - json_str = json_str.substr(json_start, json_end - json_start + 1); - } else { - json_str = json_str.substr(json_start) + "}"; - } - - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) { - res.ret_code = -3; - res.err_msg = "JSON parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - - LOG(INFO) << "robotTaskResume success, ret_code=" << res.ret_code; - } - - - - // ============================================================================ - // 3001 - robot_task_pause_req 暂停当前导航任务 (控制端口 19205) - // ============================================================================ - void AgvSrc1100::robotTaskPause(RobotTaskPauseRes& res) { - int sock = sock_nav_; // 使用控制端口 - if (sock < 0) { - res.ret_code = -1; - res.err_msg = "control socket disconnected"; - return; - } - - // 业务负载长度为 0（无 JSON 数据） - uint32_t payload_len = 0; - - // 标准 16 字节帧头（长度 4 字节，命令码 0x0BB9，保留 6 字节） - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x01, - static_cast((payload_len >> 24) & 0xFF), - static_cast((payload_len >> 16) & 0xFF), - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x0B, 0xB9, // 命令码 3001 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - setSocketTimeout(sock, 1000); // 安全超时 - - // 发送帧头（带重试） - int total_sent = 0; - int retry = 3; - while (total_sent < 16 && retry-- > 0) { - int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); - if (ret <= 0) { - usleep(10000); - continue; - } - total_sent += ret; - } - if (total_sent != 16) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "send header failed"; - return; - } - - // 接收响应（先读 16 字节帧头，再按长度读 JSON） - uint8_t recv_header[16]; - size_t header_read = 0; - while (header_read < 16) { - int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv header failed or timeout"; - return; - } - header_read += ret; - } - - // 解析数据区长度（大端 4 字节） - uint32_t data_len = (recv_header[4] << 24) | - (recv_header[5] << 16) | - (recv_header[6] << 8) | - recv_header[7]; - - // 读取数据区（JSON） - std::string json_str; - if (data_len > 0) { - json_str.resize(data_len); - size_t total_read = 0; - while (total_read < data_len) { - int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); - if (ret <= 0) { - setSocketTimeout(sock, 0); - res.ret_code = -2; - res.err_msg = "recv payload failed or timeout"; - return; - } - total_read += ret; - } - } - setSocketTimeout(sock, 0); - - if (json_str.empty()) { - res.ret_code = -4; - res.err_msg = "empty json data"; - return; - } - - // 容错提取 JSON（可能包含前缀/后缀） - size_t json_start = json_str.find('{'); - if (json_start == std::string::npos) { - res.ret_code = -4; - res.err_msg = "no JSON found in payload"; - return; - } - size_t json_end = json_str.rfind('}'); - if (json_end != std::string::npos) { - json_str = json_str.substr(json_start, json_end - json_start + 1); - } else { - json_str = json_str.substr(json_start) + "}"; - } - - // 解析 JSON - json root; - std::string parse_err; - if (!parseJson(json_str, root, parse_err)) { - res.ret_code = -3; - res.err_msg = "JSON parse error: " + parse_err; - return; - } - - res.ret_code = root.value("ret_code", 0); - res.create_on = root.value("create_on", ""); - res.err_msg = root.value("err_msg", ""); - - LOG(INFO) << "robotTaskPause success, ret_code=" << res.ret_code; - } - - - - - - // -------------------------------------------------------------------------- - // 4010 - robot_config_uploadmap_req 上传地图 - // 功能：将地图文件上传到机器人 - // -------------------------------------------------------------------------- - void AgvSrc1100::uploadMap(UploadMapResult& info, const std::string& map_json) - { - int sock = sock_config_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 清空残留数据，设置超时 - flushSocket(sock); - setSocketTimeout(sock, 1000); - - // 计算负载长度（地图 JSON 数据） - uint16_t payload_len = static_cast(map_json.size()); - // 命令码 0x0FAA = 4010 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x00, - 0x00, 0x00, - static_cast((payload_len >> 8) & 0xFF), - static_cast(payload_len & 0xFF), - 0x0F, 0xAA, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - // 发送帧头（带重试） - int totalSent = 0; - int retries = 3; - while (totalSent < 16 && retries-- > 0) - { - int ret = send(sock, send_buf + totalSent, 16 - totalSent, 0); - if (ret <= 0) - { - usleep(10000); - continue; - } - totalSent += ret; - } - if (totalSent != 16) - { - setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; - } - - // 分片发送地图数据（4096 字节一片） - const int SLICE_SIZE = 4096; - size_t offset = 0; - size_t totalSize = map_json.size(); - const char* data = map_json.c_str(); - - while (offset < totalSize) - { - int sendLen = std::min((size_t)SLICE_SIZE, totalSize - offset); - int ret = send(sock, data + offset, sendLen, 0); - if (ret <= 0) - { - setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send map payload failed"; - return; - } - offset += ret; - usleep(500); // 短暂延时，防止发送过快 - } - - // 接收响应 - const int RECV_BUF_LEN = 65536; - std::vector recvBuf(RECV_BUF_LEN); - std::string rawData; - int recvRet = 0; - while ((recvRet = recv(sock, recvBuf.data(), RECV_BUF_LEN, 0)) > 0) - { - rawData.append(reinterpret_cast(recvBuf.data()), recvRet); - } - setSocketTimeout(sock, 0); - - if (rawData.empty()) - { - info.ret_code = -2; - info.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t jsonStart = rawData.find('{'); - size_t jsonEnd = rawData.rfind('}'); - std::string json_str; - if (jsonStart == std::string::npos) - { - info.ret_code = -4; - info.err_msg = "json not found"; - return; - } - if (jsonEnd == std::string::npos) - { - json_str = rawData.substr(jsonStart) + "}"; - } - else - { - json_str = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = "json parse error: " + err; - return; - } - - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - } - - - // -------------------------------------------------------------------------- - // 4011 - robot_config_downloadmap_req 下载地图 - // 功能：从机器人下载指定地图文件 - // -------------------------------------------------------------------------- - void AgvSrc1100::downloadMap(DownloadMapResult& info, const std::string& map_name) - { - int sock = sock_config_; - if (sock < 0) - { - info.ret_code = -1; - info.err_msg = "socket not connected"; - return; - } - - // 组装请求 JSON: {"map_name":"xxx"} - std::string req_json = R"({"map_name":")" + map_name + R"("})"; - uint8_t payload_len = static_cast(req_json.size()); - - // 命令码 0x0FAB = 4011 - uint8_t send_buf[16] = { - 0x5A, 0x01, 0x00, 0x00, - 0x00, 0x00, 0x00, payload_len, - 0x0F, 0xAB, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - }; - - flushSocket(sock); - - // 发送帧头 - if (send(sock, send_buf, 16, MSG_NOSIGNAL) != 16) - { - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; - } - // 发送 JSON 负载 - if (send(sock, req_json.data(), req_json.size(), MSG_NOSIGNAL) != (ssize_t)req_json.size()) - { - info.ret_code = -2; - info.err_msg = "send payload failed"; - return; - } - - // 接收地图数据（地图文件可能较大，使用 65536 字节缓冲区） - const int RECV_BUF_LEN = 65536; - std::vector recvBuf(RECV_BUF_LEN); - std::string rawData; - int ret = 0; - - setSocketTimeout(sock, 500); // 延长超时适配大地图 - while ((ret = recv(sock, recvBuf.data(), RECV_BUF_LEN, 0)) > 0) - { - rawData.append(reinterpret_cast(recvBuf.data()), ret); - } - setSocketTimeout(sock, 0); - - if (rawData.empty()) - { - info.ret_code = -2; - info.err_msg = "no response data"; - return; - } - - // 提取并解析 JSON - size_t jsonStart = rawData.find('{'); - size_t jsonEnd = rawData.rfind('}'); - std::string json_str; - - if (jsonStart == std::string::npos) - { - info.ret_code = -4; - info.err_msg = "json not found"; - return; - } - if (jsonEnd == std::string::npos) - { - json_str = rawData.substr(jsonStart) + "}"; - } - else - { - json_str = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); - } - - json root; - std::string err; - if (!parseJson(json_str, root, err)) - { - info.ret_code = -3; - info.err_msg = err; - return; - } - - info.map_name = map_name; - info.ret_code = root.value("ret_code", 0); - info.create_on = root.value("create_on", ""); - info.err_msg = root.value("err_msg", ""); - // 成功时保存地图内容 - info.map_content = (0 == info.ret_code) ? json_str : ""; - } - - - // ============================================================================ - // 端口 19210 – 其他 API（允许 5 个连接） - // 功能：外设控制（顶升/货叉/辊筒/音频/IO 等） - // 当前未实现具体方法，预留扩展 - // ============================================================================ - - - // ============================================================================ - // 端口 19301 – 机器人推送 API（允许 10 个连接） - // 功能：接收机器人主动推送的实时状态数据 - // 当前未实现具体方法，预留扩展 - // ============================================================================ - - - - -} \ No newline at end of file diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index 09b774ff..93e0eb20 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -58,7 +58,6 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: - AbstractBiohead() = default; explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {} ~AbstractBiohead() override = default; diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 08c0953f..52b34d5d 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -7,7 +7,7 @@ #include #include #include -#include "cmvr/config/biohead_config/biohead_config.pb.h" + namespace cmvr::device { struct ServoChannel { @@ -18,7 +18,6 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const XmlNode &config); - BioHeadRobot(const cmvr::config::BioHeadConfig& config); ~BioHeadRobot() override = default; // 接口实现 @@ -67,7 +66,7 @@ namespace cmvr::device { std::shared_ptr speak_thread_; std::atomic speak_running_{false}; - cmvr::config::BioHeadConfig config_; + }; diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index 8ff9e3b3..c3b67887 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -26,219 +26,6 @@ BioHeadRobot::BioHeadRobot(const XmlNode &config) : AbstractBiohead(config) { } -BioHeadRobot::BioHeadRobot(const cmvr::config::BioHeadConfig& config) : config_(config) { - // 从config中读取串口参数 - port_name_ = config_.serial(); - - serial_ = std::make_shared(); - if (!serial_->open(port_name_, 115200)) { - LOG(ERROR) << "[BioHeadRobot] Failed to open serial port: " << port_name_; - } else { - serial_->wakeupESP32(port_name_); - LOG(INFO) << "[BioHeadRobot] Serial port opened and ESP32 woken up."; - } - - // 解析眉毛配置 - if (config_.has_eyebrow()) { - const auto& eyebrow = config_.eyebrow(); - std::string serial_str = eyebrow.serial(); - std::string offset_str = eyebrow.offest(); - std::string min_angles_str = eyebrow.jlmtlow(); - std::string max_angles_str = eyebrow.jlmtup(); - - // 解析偏移角 - std::vector offsets; - std::istringstream oss(offset_str); - double val; - while (oss >> val) offsets.push_back(val); - - // 解析角度限制 - std::vector min_angles; - if (!min_angles_str.empty()) { - std::istringstream min_ss(min_angles_str); - while (min_ss >> val) min_angles.push_back(val); - } - - std::vector max_angles; - if (!max_angles_str.empty()) { - std::istringstream max_ss(max_angles_str); - while (max_ss >> val) max_angles.push_back(val); - } - - // 解析 channel，如 64:0~3 - size_t colon = serial_str.find(':'); - if (colon != std::string::npos) { - uint8_t addr = std::stoi(serial_str.substr(0, colon)); - std::string range = serial_str.substr(colon + 1); - size_t tilde = range.find('~'); - if (tilde != std::string::npos) { - int ch_start = std::stoi(range.substr(0, tilde)); - int ch_end = std::stoi(range.substr(tilde + 1)); - int channel_count = ch_end - ch_start + 1; - - // 添加舵机通道 - for (int ch = ch_start; ch <= ch_end; ++ch) { - channels_.push_back({addr, static_cast(ch)}); - } - - // 添加角度限制 - if (min_angles.size() == 1) { - min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); - } else if (min_angles.size() >= channel_count) { - min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); - } else { - min_angles_.insert(min_angles_.end(), channel_count, 0.0); - } - - if (max_angles.size() == 1) { - max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); - } else if (max_angles.size() >= channel_count) { - max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); - } else { - max_angles_.insert(max_angles_.end(), channel_count, 180.0); - } - } - } - } - - // 解析眼睛配置 - if (config_.has_eye()) { - const auto& eye = config_.eye(); - std::string serial_str = eye.serial(); - std::string offset_str = eye.offest(); - std::string min_angles_str = eye.jlmtlow(); - std::string max_angles_str = eye.jlmtup(); - - // 解析偏移角 - std::vector offsets; - std::istringstream oss(offset_str); - double val; - while (oss >> val) offsets.push_back(val); - - // 解析角度限制 - std::vector min_angles; - if (!min_angles_str.empty()) { - std::istringstream min_ss(min_angles_str); - while (min_ss >> val) min_angles.push_back(val); - } - - std::vector max_angles; - if (!max_angles_str.empty()) { - std::istringstream max_ss(max_angles_str); - while (max_ss >> val) max_angles.push_back(val); - } - - // 解析 channel，如 64:4~9 - size_t colon = serial_str.find(':'); - if (colon != std::string::npos) { - uint8_t addr = std::stoi(serial_str.substr(0, colon)); - std::string range = serial_str.substr(colon + 1); - size_t tilde = range.find('~'); - if (tilde != std::string::npos) { - int ch_start = std::stoi(range.substr(0, tilde)); - int ch_end = std::stoi(range.substr(tilde + 1)); - int channel_count = ch_end - ch_start + 1; - - // 添加舵机通道 - for (int ch = ch_start; ch <= ch_end; ++ch) { - channels_.push_back({addr, static_cast(ch)}); - } - - // 添加角度限制 - if (min_angles.size() == 1) { - min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); - } else if (min_angles.size() >= channel_count) { - min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); - } else { - min_angles_.insert(min_angles_.end(), channel_count, 0.0); - } - - if (max_angles.size() == 1) { - max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); - } else if (max_angles.size() >= channel_count) { - max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); - } else { - max_angles_.insert(max_angles_.end(), channel_count, 180.0); - } - } - } - } - - // 解析嘴巴配置 - if (config_.has_mouth()) { - const auto& mouth = config_.mouth(); - std::string serial_str = mouth.serial(); - std::string offset_str = mouth.offest(); - std::string min_angles_str = mouth.jlmtlow(); - std::string max_angles_str = mouth.jlmtup(); - - // 解析偏移角 - std::vector offsets; - std::istringstream oss(offset_str); - double val; - while (oss >> val) offsets.push_back(val); - - // 解析角度限制 - std::vector min_angles; - if (!min_angles_str.empty()) { - std::istringstream min_ss(min_angles_str); - while (min_ss >> val) min_angles.push_back(val); - } - - std::vector max_angles; - if (!max_angles_str.empty()) { - std::istringstream max_ss(max_angles_str); - while (max_ss >> val) max_angles.push_back(val); - } - - // 解析 channel，如 65:0~9 - size_t colon = serial_str.find(':'); - if (colon != std::string::npos) { - uint8_t addr = std::stoi(serial_str.substr(0, colon)); - std::string range = serial_str.substr(colon + 1); - size_t tilde = range.find('~'); - if (tilde != std::string::npos) { - int ch_start = std::stoi(range.substr(0, tilde)); - int ch_end = std::stoi(range.substr(tilde + 1)); - int channel_count = ch_end - ch_start + 1; - - // 添加舵机通道 - for (int ch = ch_start; ch <= ch_end; ++ch) { - channels_.push_back({addr, static_cast(ch)}); - } - - // 添加角度限制 - if (min_angles.size() == 1) { - min_angles_.insert(min_angles_.end(), channel_count, min_angles[0]); - } else if (min_angles.size() >= channel_count) { - min_angles_.insert(min_angles_.end(), min_angles.begin(), min_angles.begin() + channel_count); - } else { - min_angles_.insert(min_angles_.end(), channel_count, 0.0); - } - - if (max_angles.size() == 1) { - max_angles_.insert(max_angles_.end(), channel_count, max_angles[0]); - } else if (max_angles.size() >= channel_count) { - max_angles_.insert(max_angles_.end(), max_angles.begin(), max_angles.begin() + channel_count); - } else { - max_angles_.insert(max_angles_.end(), channel_count, 180.0); - } - } - } - } - - // 确保所有向量大小一致 - if (min_angles_.size() != channels_.size()) { - min_angles_.resize(channels_.size(), 0.0); - } - - if (max_angles_.size() != channels_.size()) { - max_angles_.resize(channels_.size(), 180.0); - } - - current_joints_.resize(channels_.size(), 90.0); - last_joints_ = current_joints_; -} void BioHeadRobot::parseXmlConfig(const XmlNode &node) { diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index 6fce7f5e..ca0399d0 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -54,7 +54,6 @@ namespace cmvr::device { class AbstractCanbus : public AbstractDevice { public: - AbstractCanbus() = default; AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {} ~AbstractCanbus() {} /** diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index 28779a06..aef0af2c 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -35,17 +35,6 @@ namespace cmvr { // LOG(ERROR) << "[SocketCanClientRaw] ([SocketCanClientRaw]): Failed to parse XML: " << e.what(); } - SocketCanClientRaw::SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config):config_(config) - { - auto channel_id = config_.channelid(); - port_ = static_cast(channel_id); - interface_ = CANCardParameter::NATIVE; - - enable_can_err_check_ = false; - - - } - void SocketCanClientRaw::init() { // if (!parameter.has_channel_id()) { diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 04d57c86..3de5b42c 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -26,7 +26,6 @@ #include "gflags/gflags.h" #include "../../abstract_canbus.h" #include "canbus/common/canbus_consts.h" -#include "cmvr/config/robot_config/robot_config.pb.h" namespace cmvr { namespace device { @@ -37,7 +36,6 @@ namespace cmvr { class SocketCanClientRaw final : public AbstractCanbus { public: explicit SocketCanClientRaw(const XmlNode &cfg); - SocketCanClientRaw(const cmvr::config::CanDeviceConfig& config); ~SocketCanClientRaw(); void init() override; void start() override; @@ -75,8 +73,6 @@ namespace cmvr { // bool enable_can_err_check_{false}; - - cmvr::config::CanDeviceConfig config_; }; } } diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 3b7669c9..532924d2 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(ti5_motor) -add_subdirectory(eyou_motor) + # -------------------------------------------------------- # Unit test # -------------------------------------------------------- diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index bb89a415..710f88d2 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -41,7 +41,6 @@ namespace cmvr::device{ } JointStatus; public: - AbstractMotor() = default; explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {} AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {} ~AbstractMotor() override = default; diff --git a/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt b/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt deleted file mode 100644 index 9f00953a..00000000 --- a/cmvr-es/devices/motor/eyou_motor/CMakeLists.txt +++ /dev/null @@ -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) \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp deleted file mode 100644 index 26636505..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.cpp +++ /dev/null @@ -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 -#include -#include - -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>(); - - // 创建 EcMaster 实例 - master_ = std::make_unique(); - - // 初始化主站 - 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 rxPdos = config.availableRxPdos; - std::vector 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, 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(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(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(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((std::abs(u_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); - auto decel = static_cast((std::abs(l_qdd) / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); - - // 通过SDO写入加速度和减速度限制 - if (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(r / 2); - if (pos > temp || pos < -temp) { - pos = pos % static_cast(r); - if (pos > temp || pos < -temp) { - if (pos > temp) { - pos = pos - static_cast(r); - } else if (pos < -temp) { - pos = pos + static_cast(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(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(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((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 EYouEthercatProtocol::GetRobotDetail() { - if (message_manager_) { - auto data_ptr = std::make_unique(); - message_manager_->GetSensorData(data_ptr.get()); - return data_ptr; - } - return std::make_unique(); -} - -// 辅助函数实现（弧度制版本） -double EYouEthercatProtocol::posToAngle(int32_t pos) const { - // 脉冲数 -> 弧度 - // 圈数 = 脉冲数 / (GearRatio * PULSE_PER_REV) - // 弧度 = 圈数 * 2π - return (pos * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV); -} - -int32_t EYouEthercatProtocol::angleToPos(double angle) const { - // 弧度 -> 脉冲数 - // 圈数 = 弧度 / 2π - // 脉冲数 = 圈数 * GearRatio * PULSE_PER_REV - return static_cast((angle / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV); -} - -double EYouEthercatProtocol::velToSpeed(double vel) const { - // vel: rad/s (角速度) - // 输出：电机速度（脉冲/秒） - // 圈数/秒 = vel / 2π - // 脉冲/秒 = 圈数/秒 * GearRatio * PULSE_PER_REV - return (vel / (2.0 * M_PI)) * GearRatio * PULSE_PER_REV; -} - -double EYouEthercatProtocol::speedToVel(int32_t speed) const { - // speed: 电机速度（脉冲/秒） - // 输出：rad/s - // 圈数/秒 = speed / (GearRatio * PULSE_PER_REV) - // 弧度/秒 = 圈数/秒 * 2π - return (speed * 2.0 * M_PI) / (GearRatio * PULSE_PER_REV); -} - -bool EYouEthercatProtocol::waitUntil(std::function condition, int timeout_ms) { - auto start = std::chrono::steady_clock::now(); - while (!condition()) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms)) { - return false; - } - } - return true; -} - -void EYouEthercatProtocol::updateNodeState(uint8_t node_id) { - - // 更新状态字 - node_states_[node_id].status_word = master_->getStatusWord(node_id); - // 可以根据需要更新其他状态信息 -} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h b/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h deleted file mode 100644 index 2b14279c..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/eyou_motor_ethercat_protocol.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include - -namespace cmvr { - namespace device { - - // 节点状态结构体 - struct NodeState { - bool enabled = false; - bool initialized = false; - msgs::RunMode current_mode = msgs::RUN_MODE_UNSPECIFIED; - uint16_t status_word = 0; - int32_t position_offset = 0; - 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 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 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 node_states_; - std::unordered_map last_Qd_; - std::unordered_map last_Qdd_; - - std::shared_ptr> can_sender_; - std::shared_ptr> message_manager_; - - std::string interface_name_; - std::string xml_config_path_; - bool initialized_; - int slave_count_; - - std::unique_ptr master_; - std::unique_ptr parser_; - EcSlaveConfig slave_config_; - bool use_fixed_pdo_; - - std::mutex master_mutex_; - }; - - } // namespace device -} // namespace cmvr \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp deleted file mode 100644 index afac1e5c..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.cpp +++ /dev/null @@ -1,1143 +0,0 @@ -// ec_master.cpp -#include "ec_master.h" -#include - -static float pgain = 0.01f; -static float igain = 0.00002f; -static int64_t syncoffset = 500000; -static int64_t timeerror = 0; - -static void add_time_ns(ec_timet* ts, int64_t addtime) { - ec_timet addts; - addts.tv_nsec = addtime % 1000000000; - addts.tv_sec = (addtime - addts.tv_nsec) / 1000000000; - osal_timespecadd(ts, &addts, ts); -} - -// DC 同步 PI 计算 -static void ec_sync(int64_t reftime, int64_t cycletime, int64_t *offsettime) { - static int64_t integral = 0; - int64_t delta; - delta = (reftime - syncoffset) % cycletime; - if (delta > (cycletime / 2)) { - delta = delta - cycletime; - } - timeerror = -delta; - integral += timeerror; - *offsettime = (int64_t)((timeerror * pgain) + (integral * igain)); -} - -// ==================== 构造和析构 ==================== - -EcMaster::EcMaster() { - memset(&ctx, 0, sizeof(ctx)); - memset(IOmap, 0, sizeof(IOmap)); - - slaveCount = 0; - expectedWKC = 0; - currentgroup = 0; - inOp = false; - isRunning = false; - mappingdone = 0; - dorun = 0; - cyclicWkcErrorCount = 0; - cycleTimeNs = 1000000; - cyclicThread = nullptr; - checkThread = nullptr; - anyPdoDirty = false; -} - -EcMaster::~EcMaster() { - shutdown(); - // shadowBuffers 的析构函数会自动清理内存 -} - -// ==================== 初始化和状态控制 ==================== - -int EcMaster::init(const char* ifName, int cycleTimeUs) { - cycleTimeNs = cycleTimeUs * 1000; - - if (!ecx_init(&ctx, ifName)) { - printf("Failed to init EtherCAT on %s\n", ifName); - return -EC_FAILED_INIT; - } - - ecx_config_init(&ctx); - slaveCount = ctx.slavecount; - - if (slaveCount <= 0) { - printf("No slave found\n"); - return -EC_FAILED_NOSLAVE; - } - - printf("%d slaves found\n", slaveCount); - - isRunning = true; - mappingdone = 0; - dorun = 0; - inOp = false; - cyclicWkcErrorCount = 0; - anyPdoDirty = false; - - osal_thread_create_rt(&cyclicThread, 128000, reinterpret_cast(cyclicThreadFunc), this); - - return slaveCount; -} - -int EcMaster::setState(uint16_t slaveId, uint8_t targetState) { - if (slaveId > (uint16_t)ctx.slavecount) return -1; - - int oldManual = ctx.manualstatechange; - ctx.manualstatechange = 1; - - ctx.slavelist[slaveId].state = targetState; - ecx_writestate(&ctx, slaveId); - - int ret = ecx_statecheck(&ctx, slaveId, targetState, EC_TIMEOUTSTATE); - ctx.manualstatechange = oldManual; - - if (ret != targetState) { - printf("Slave %d failed to enter state %d (current: %d)\n", slaveId, targetState, ret); - return -1; - } - - return 0; -} - -int EcMaster::gotoOperational() { - while (!mappingdone) { - osal_usleep(100); - } - - dorun = 1; - - printf("Waiting for PDO exchange to stabilize...\n"); - osal_usleep(500000); - - // ========== 逐个处理每个从站 ========== - int allOperational = 1; - for (int i = 1; i <= ctx.slavecount; i++) { - printf("\n--- Processing slave %d ---\n", i); - - // 1. 检查并清除错误状态 - if (ctx.slavelist[i].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) { - printf(" Clearing error on slave %d\n", i); - ctx.slavelist[i].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ecx_writestate(&ctx, i); - osal_usleep(100000); - - // 刷新状态 - ecx_readstate(&ctx); - printf(" Slave %d state after error clear: 0x%02X\n", i, ctx.slavelist[i].state); - } - - // 2. 请求进入 OP 状态 - printf(" Requesting OPERATIONAL state for slave %d...\n", i); - ctx.slavelist[i].state = EC_STATE_OPERATIONAL; - ecx_writestate(&ctx, i); - - // 3. 等待该从站进入 OP 状态 - printf(" Waiting for slave %d to enter OPERATIONAL...\n", i); - - int retry = 0; - const int maxRetry = 200; // 最多等待2秒 (200 * 10ms) - int currentState = 0; - - while (retry < maxRetry) { - osal_usleep(10000); // 10ms - ecx_readstate(&ctx); - currentState = ctx.slavelist[i].state; - - if (currentState == EC_STATE_OPERATIONAL) { - printf(" ✓ Slave %d entered OPERATIONAL after %d ms\n", i, retry * 10); - break; - } - - // 如果状态异常，提前退出 - if (currentState == EC_STATE_INIT || currentState == EC_STATE_PRE_OP) { - printf(" ✗ Slave %d regressed to state 0x%02X\n", i, currentState); - break; - } - - retry++; - } - - // 4. 检查结果 - if (currentState == EC_STATE_OPERATIONAL) { - printf(" ✓ Slave %d is OPERATIONAL\n", i); - } else { - printf(" ✗ Slave %d failed to enter OPERATIONAL, state: 0x%02X\n", i, currentState); - - // 读取错误码 - uint16_t alStatusCode = 0; - int psize = 2; - ecx_SDOread(&ctx, i, 0x0134, 0, FALSE, &psize, &alStatusCode, EC_TIMEOUTRXM); - printf(" AL Status Code: 0x%04X\n", alStatusCode); - - allOperational = 0; - } - } - - // 5. 检查是否所有从站都进入了 OP - if (!allOperational) { - printf("\n✗ Failed to enter OPERATIONAL\n"); - return -1; - } - - printf("\n✓ All slaves are OPERATIONAL\n"); - - // 6. 启动 check 线程 - printf("Starting check thread for runtime monitoring...\n"); - osal_thread_create(&checkThread, 128000, reinterpret_cast(checkThreadFunc), this); - - osal_usleep(200000); - inOp = true; - printf("EtherCAT OPERATIONAL (all slaves)\n"); - return 0; -} - -void EcMaster::shutdown() { - printf("Shutting down...\n"); - - if (inOp) { - inOp = false; - dorun = 0; - osal_usleep(100000); - } - - for (int i = 1; i <= slaveCount; i++) { - printf(" Resetting slave %d to INIT...\n", i); - ctx.slavelist[i].state = EC_STATE_INIT; - ecx_writestate(&ctx, i); - osal_usleep(50000); - ecx_statecheck(&ctx, i, EC_STATE_INIT, 500000); - } - - isRunning = false; - osal_usleep(100000); - - shadowBuffers.clear(); - - printf("Shutdown complete\n"); -} - -// ==================== 影子缓冲区管理 ==================== - -EcMaster::SlaveShadowBuffer* EcMaster::getShadowBuffer(uint16_t slaveId) { - auto it = shadowBuffers.find(slaveId); - if (it != shadowBuffers.end()) { - return &it->second; - } - return nullptr; -} - -void EcMaster::syncSlaveOutputs(uint16_t slaveId) { - auto* shadow = getShadowBuffer(slaveId); - if (!shadow || !shadow->dirty || !shadow->data) return; - - auto outIt = outputs.find(slaveId); - auto sizeIt = outputSizes.find(slaveId); - if (outIt == outputs.end() || outIt->second == nullptr) return; - if (sizeIt == outputSizes.end()) return; - - size_t copySize = std::min(shadow->size, (size_t)sizeIt->second); - memcpy(outIt->second, shadow->data, copySize); - shadow->dirty = false; -} - -void EcMaster::syncAllOutputs() { - if (!anyPdoDirty.load()) return; - - for (auto& pair : shadowBuffers) { - uint16_t slaveId = pair.first; - SlaveShadowBuffer& shadow = pair.second; - - if (!shadow.dirty || !shadow.data) continue; - - auto outIt = outputs.find(slaveId); - auto sizeIt = outputSizes.find(slaveId); - if (outIt == outputs.end() || outIt->second == nullptr) continue; - if (sizeIt == outputSizes.end()) continue; - - size_t copySize = std::min(shadow.size, (size_t)sizeIt->second); - memcpy(outIt->second, shadow.data, copySize); - shadow.dirty = false; - } - - anyPdoDirty = false; -} - -// ==================== 线程函数 ==================== - -OSAL_THREAD_FUNC_RT EcMaster::cyclicThreadFunc(void* arg) { - static_cast(arg)->runCyclic(); -} - -OSAL_THREAD_FUNC EcMaster::checkThreadFunc(void* arg) { - static_cast(arg)->runCheck(); -} - -void EcMaster::runCyclic() { - ec_timet ts; - int ht; - static int64_t toff = 0; - int cycleCount = 0; - int printInterval = 500; - - printf("[Cyclic Thread] Thread started, waiting for mappingdone...\n"); - - while (!mappingdone && isRunning) { - osal_usleep(100); - } - - if (!isRunning) { - printf("[Cyclic Thread] Exiting - master not running\n"); - return; - } - - printf("[Cyclic Thread] Mapping done, initializing timing...\n"); - - osal_get_monotonic_time(&ts); - ht = (ts.tv_nsec / 1000000) + 1; - ts.tv_nsec = ht * 1000000; - - printf("[Cyclic Thread] Entering main cyclic loop (cycleTime=%lld ns)\n", - (long long)cycleTimeNs); - - int receiveSuccessCount = 0; - int receiveFailCount = 0; - int lastPrintCycle = 0; - - ecx_send_processdata(&ctx); - - while (isRunning) { - add_time_ns(&ts, cycleTimeNs + toff); - osal_monotonic_sleep(&ts); - - cycleCount++; - - if (dorun) { - // ========== 同步所有从站的影子缓冲区到 outputs ========== - if (anyPdoDirty.load()) { - for (auto& pair : shadowBuffers) { - uint16_t slaveId = pair.first; - SlaveShadowBuffer& shadow = pair.second; - - if (!shadow.dirty || !shadow.data) continue; - - auto outIt = outputs.find(slaveId); - auto sizeIt = outputSizes.find(slaveId); - if (outIt == outputs.end() || outIt->second == nullptr) continue; - if (sizeIt == outputSizes.end()) continue; - - size_t copySize = std::min(shadow.size, (size_t)sizeIt->second); - memcpy(outIt->second, shadow.data, copySize); - shadow.dirty = false; - } - anyPdoDirty = false; - } - - ecx_send_processdata(&ctx); - - int wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - - if (wkc != expectedWKC) { - cyclicWkcErrorCount++; - receiveFailCount++; - if (cyclicWkcErrorCount <= 5 || cyclicWkcErrorCount % 100 == 0) { - printf("[Cyclic] WARNING: WKC mismatch! Expected=%d, Got=%d, ErrorCount=%d\n", - expectedWKC, wkc, cyclicWkcErrorCount.load()); - } - } else { - receiveSuccessCount++; - } - - if (ctx.slavelist[0].hasdc && (wkc > 0)) { - ec_sync(ctx.DCtime, cycleTimeNs, &toff); - } - - ecx_mbxhandler(&ctx, 0, 4); - - // if (cycleCount - lastPrintCycle >= printInterval) { - // lastPrintCycle = cycleCount; - // - // printf("\n========== PDO Data Exchange Status ==========\n"); - // printf("Cycle: %d | Success: %d | Fail: %d | WKC Error: %d\n", - // cycleCount, receiveSuccessCount, receiveFailCount, cyclicWkcErrorCount.load()); - // printf("DC time: %lld | toff: %lld\n", (long long)ctx.DCtime, (long long)toff); - // - // for (int slaveIdx = 1; slaveIdx <= slaveCount; slaveIdx++) { - // ec_slavet* slave = &ctx.slavelist[slaveIdx]; - // - // printf("\n[Slave %d] State: 0x%02X | AL Status: 0x%04X\n", - // slaveIdx, slave->state, slave->ALstatuscode); - // - // if (slave->inputs && slave->Ibytes > 0) { - // printf(" Inputs (%d bytes): ", slave->Ibytes); - // for (int i = 0; i < slave->Ibytes && i < 16; i++) { - // printf("%02X ", ((uint8_t*)slave->inputs)[i]); - // } - // if (slave->Ibytes > 16) printf("..."); - // printf("\n"); - // - // if (slave->Ibytes >= 2) { - // uint16_t statusWord = *(uint16_t*)slave->inputs; - // printf(" -> Status Word: 0x%04X", statusWord); - // if (statusWord == 0xFFFF) { - // printf(" (INVALID - All ones)"); - // } else if (statusWord == 0) { - // printf(" (INVALID - All zeros)"); - // } else { - // printf(" ["); - // if (statusWord & 0x0001) printf("ReadyToSwitchOn "); - // if (statusWord & 0x0002) printf("SwitchedOn "); - // if (statusWord & 0x0004) printf("OperationEnabled "); - // if (statusWord & 0x0008) printf("Fault "); - // if (statusWord & 0x0010) printf("VoltageEnabled "); - // if (statusWord & 0x0020) printf("QuickStop "); - // if (statusWord & 0x0040) printf("SwitchOnDisabled "); - // if (statusWord & 0x0080) printf("Warning "); - // printf("]"); - // } - // printf("\n"); - // - // if (slave->Ibytes >= 6) { - // int32_t actualPosition = *(int32_t*)(slave->inputs + 2); - // printf(" -> Actual Position: %d\n", actualPosition); - // } - // } - // } - // - // if (slave->outputs && slave->Obytes > 0) { - // printf(" Outputs (%d bytes): ", slave->Obytes); - // for (int i = 0; i < slave->Obytes && i < 16; i++) { - // printf("%02X ", ((uint8_t*)slave->outputs)[i]); - // } - // if (slave->Obytes > 16) printf("..."); - // printf("\n"); - // - // if (slave->Obytes >= 2) { - // uint16_t controlWord = *(uint16_t*)slave->outputs; - // printf(" <- Control Word: 0x%04X\n", controlWord); - // } - // } - // } - // - // printf("================================================\n\n"); - // - // receiveSuccessCount = 0; - // receiveFailCount = 0; - // } - - } else { - if (cycleCount % 1000 == 0) { - printf("[Cyclic] Waiting for dorun=1, cycle=%d\n", cycleCount); - } - } - } - - printf("[Cyclic Thread] Thread exiting\n"); -} - -void EcMaster::runCheck() { - while (isRunning) { - if (dorun && (cyclicWkcErrorCount > 2 || ctx.grouplist[currentgroup].docheckstate)) { - ctx.grouplist[currentgroup].docheckstate = FALSE; - ecx_readstate(&ctx); - - for (int slaveix = 1; slaveix <= ctx.slavecount; slaveix++) { - ec_slavet *slave = &ctx.slavelist[slaveix]; - - if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL)) { - ctx.grouplist[currentgroup].docheckstate = TRUE; - - if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) { - printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix); - slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ecx_writestate(&ctx, slaveix); - } - else if (slave->state == EC_STATE_SAFE_OP) { - printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix); - slave->state = EC_STATE_OPERATIONAL; - if (slave->mbxhandlerstate == ECT_MBXH_LOST) - slave->mbxhandlerstate = ECT_MBXH_CYCLIC; - ecx_writestate(&ctx, slaveix); - } - else if (slave->state > EC_STATE_NONE) { - if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP) { - slave->islost = FALSE; - printf("MESSAGE : slave %d reconfigured\n", slaveix); - } - } - else if (!slave->islost) { - ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); - if (slave->state == EC_STATE_NONE) { - slave->islost = TRUE; - slave->mbxhandlerstate = ECT_MBXH_LOST; - if (slave->Ibytes) { - memset(slave->inputs, 0x00, slave->Ibytes); - } - printf("ERROR : slave %d lost\n", slaveix); - } - } - } - - if (slave->islost) { - if (slave->state <= EC_STATE_INIT) { - if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON)) { - slave->islost = FALSE; - printf("MESSAGE : slave %d recovered\n", slaveix); - } - } - else { - slave->islost = FALSE; - printf("MESSAGE : slave %d found\n", slaveix); - } - } - } - - if (!ctx.grouplist[currentgroup].docheckstate) { - printf("OK : all slaves resumed OPERATIONAL.\n"); - } - } - - osal_usleep(10000); - } -} - -// ==================== PDO 配置 ==================== - -int EcMaster::configurePdo(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo) { - uint8_t zero = 0; - int wkc = 0; - - // ========== 步骤1: 进入 PRE-OP ========== - printf("Step 1: Entering PRE-OP...\n"); - if (setState(slaveId, EC_STATE_PRE_OP) != 0) { - printf("Failed to enter PRE-OP\n"); - return -1; - } - printf("Slave %d in PRE-OP\n", slaveId); - osal_usleep(50000); - - // ========== 步骤2: 禁用全局 PDO 分配 ========== - printf("Step 2: Disabling global PDO assignment...\n"); - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); - printf("ecx_SDOwrite 0x1C12, wkc = %d\n", wkc); - - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); - printf("ecx_SDOwrite 0x1C13, wkc = %d\n", wkc); - - // ========== 计算输出大小（用于分配影子缓冲区） ========== - int outputSize = 0; - if (rxPdo) { - for (const auto& entry : rxPdo->entries) { - outputSize += (entry.bitlen + 7) / 8; - } - printf("Calculated output size: %d bytes\n", outputSize); - } - - // ========== 步骤3: 配置 RxPDO ========== - if (rxPdo && rxPdo->mapCount > 0) { - printf("Step 3: Configuring RxPDO 0x%04X\n", rxPdo->index); - - wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to disable RxPDO 0x%04X\n", rxPdo->index); - return -1; - } - - uint8_t subidx = 1; - for (const auto& entry : rxPdo->entries) { - uint32_t mapValue = entry.makeMapValue(); - wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, subidx++, FALSE, 4, &mapValue, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to write RxPDO entry 0x%04X:%02X\n", entry.index, entry.subindex); - return -1; - } - printf(" Entry: 0x%04X:%02X (%d bits)\n", entry.index, entry.subindex, entry.bitlen); - } - - uint8_t mapCount = rxPdo->mapCount; - wkc = ecx_SDOwrite(&ctx, slaveId, rxPdo->index, 0x00, FALSE, 1, &mapCount, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to enable RxPDO 0x%04X\n", rxPdo->index); - return -1; - } - printf("RxPDO 0x%04X configured successfully\n", rxPdo->index); - } - - // ========== 步骤4: 配置 TxPDO ========== - if (txPdo && txPdo->mapCount > 0) { - printf("Step 4: Configuring TxPDO 0x%04X\n", txPdo->index); - - wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, 0x00, FALSE, 1, &zero, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to disable TxPDO 0x%04X\n", txPdo->index); - return -1; - } - - uint8_t subidx = 1; - for (const auto& entry : txPdo->entries) { - uint32_t mapValue = entry.makeMapValue(); - wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, subidx++, FALSE, 4, &mapValue, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to write TxPDO entry 0x%04X:%02X\n", entry.index, entry.subindex); - return -1; - } - printf(" Entry: 0x%04X:%02X (%d bits)\n", entry.index, entry.subindex, entry.bitlen); - } - - uint8_t mapCount = txPdo->mapCount; - wkc = ecx_SDOwrite(&ctx, slaveId, txPdo->index, 0x00, FALSE, 1, &mapCount, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to enable TxPDO 0x%04X\n", txPdo->index); - return -1; - } - printf("TxPDO 0x%04X configured successfully\n", txPdo->index); - } - - // ========== 步骤5: 应用 PDO 分配 ========== - printf("Step 5: Applying PDO assignment...\n"); - - if (rxPdo && rxPdo->mapCount > 0) { - uint8_t rpdo_map_count = 1; - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x01, FALSE, 2, &rxPdo->index, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to write RxPDO index to 0x1C12:01\n"); - return -1; - } - - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C12, 0x00, FALSE, 1, &rpdo_map_count, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to set RxPDO count\n"); - return -1; - } - printf("RxPDO assigned to 0x1C12\n"); - } - - if (txPdo && txPdo->mapCount > 0) { - uint8_t tpdo_map_count = 1; - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x01, FALSE, 2, &txPdo->index, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to write TxPDO index to 0x1C13:01\n"); - return -1; - } - - wkc = ecx_SDOwrite(&ctx, slaveId, 0x1C13, 0x00, FALSE, 1, &tpdo_map_count, EC_TIMEOUTRXM); - if (wkc <= 0) { - printf("Failed to set TxPDO count\n"); - return -1; - } - printf("TxPDO assigned to 0x1C13\n"); - } - - // ========== 步骤6: 为这个从站分配影子缓冲区 ========== - if (outputSize > 0) { - // 移除旧的影子缓冲区（如果存在） - auto it = shadowBuffers.find(slaveId); - if (it != shadowBuffers.end()) { - shadowBuffers.erase(it); - } - // 创建新的影子缓冲区 - shadowBuffers.emplace(std::piecewise_construct, - std::forward_as_tuple(slaveId), - std::forward_as_tuple(outputSize)); - printf("Shadow buffer allocated for slave %d: %d bytes\n", slaveId, outputSize); - } - - // ========== 步骤7: 设置 PDO 偏移 ========== - setupPdoOffsets(slaveId, rxPdo, txPdo); - - printf("PDO configuration for slave %d completed\n", slaveId); - return 0; -} - -// ========== setupMappingAndDc - 只需要做映射和DC配置 ========== -int EcMaster::setupMappingAndDc() { - printf("\n========== Step 6: Finalizing mapping and DC configuration ==========\n"); - - // 配置映射 - ctx.manualstatechange = 0; - ecx_config_map_group(&ctx, IOmap, 0); - - // 设置 mappingdone 标志 - mappingdone = 1; - - // 验证并保存所有从站的 PDO 数据指针和偏移 - for (int i = 1; i <= ctx.slavecount; i++) { - inputs[i] = ctx.slavelist[i].inputs; - outputs[i] = ctx.slavelist[i].outputs; - inputSizes[i] = ctx.slavelist[i].Ibytes; - outputSizes[i] = ctx.slavelist[i].Obytes; - - if (outputs[i]) { - outputOffsets[i] = (uint8_t*)outputs[i] - IOmap; - printf("Slave %d: Out offset=%d, size=%d bytes\n", - i, outputOffsets[i], outputSizes[i]); - } - printf("Slave %d: In=%d bytes, Out=%d bytes\n", i, inputSizes[i], outputSizes[i]); - - printf(" SM2 (Outputs): StartAddr=0x%04X, Length=%d\n", - ctx.slavelist[i].SM[2].StartAddr, ctx.slavelist[i].SM[2].SMlength); - printf(" SM3 (Inputs): StartAddr=0x%04X, Length=%d\n", - ctx.slavelist[i].SM[3].StartAddr, ctx.slavelist[i].SM[3].SMlength); - } - - expectedWKC = (ctx.grouplist[0].outputsWKC * 2) + ctx.grouplist[0].inputsWKC; - printf("Expected WKC: %d\n", expectedWKC); - - // 配置分布式时钟 - ecx_configdc(&ctx); - - // 添加所有 CoE 从站到周期邮箱处理 - for (int i = 1; i <= ctx.slavecount; i++) { - if (ctx.slavelist[i].CoEdetails > 0) { - ecx_slavembxcyclic(&ctx, i); - printf("Slave %d added to cyclic mailbox handler\n", i); - } - } - - // 验证影子缓冲区大小是否匹配实际输出大小 - for (int i = 1; i <= ctx.slavecount; i++) { - auto it = shadowBuffers.find(i); - if (it != shadowBuffers.end()) { - size_t shadowSize = it->second.size; - int actualSize = outputSizes[i]; - if (shadowSize != (size_t)actualSize) { - printf("WARNING: Slave %d shadow buffer size (%zu) != actual output size (%d)\n", - i, shadowSize, actualSize); - // 重新分配匹配的大小 - it->second = SlaveShadowBuffer(actualSize); - printf(" Reallocated shadow buffer to %d bytes\n", actualSize); - } - } else if (outputSizes[i] > 0) { - // 如果没有影子缓冲区但有输出数据，创建一个 - shadowBuffers.emplace(std::piecewise_construct, - std::forward_as_tuple(i), - std::forward_as_tuple(outputSizes[i])); - printf("Created shadow buffer for slave %d: %d bytes\n", i, outputSizes[i]); - } - } - - printf("Mapping and DC configuration completed\n"); - return 0; -} - -int EcMaster::setupPdoOffsets(uint16_t slaveId, const EcPdo* rxPdo, const EcPdo* txPdo) { - EcPdoOffsets offsets; - memset(&offsets, 0xFF, sizeof(offsets)); - - // ========== 解析 RxPDO (Outputs) ========== - if (rxPdo) { - uint16_t offset = 0; - printf("RxPDO 0x%04X mapping (%zu entries):\n", rxPdo->index, rxPdo->entries.size()); - - for (const auto& entry : rxPdo->entries) { - uint8_t bytes = (entry.bitlen + 7) / 8; - printf(" Entry 0x%04X:%02X -> offset %d (%d bytes)\n", - entry.index, entry.subindex, offset, bytes); - - switch (entry.index) { - case 0x6040: // Control Word - offsets.controlWord = offset; - printf(" *** Control Word at offset %d ***\n", offset); - break; - case 0x607A: // Target Position - offsets.targetPosition = offset; - break; - case 0x60FF: // Target Velocity - offsets.targetVelocity = offset; - break; - case 0x6071: // Target Torque - offsets.targetTorque = offset; - break; - case 0x6083: // Profile Acceleration - offsets.profileAcceleration = offset; - printf(" *** Profile Acceleration at offset %d ***\n", offset); - break; - case 0x6084: // Profile Deceleration - offsets.profileDeceleration = offset; - printf(" *** Profile Deceleration at offset %d ***\n", offset); - break; - case 0x6081: // Profile Velocity - offsets.profileVelocity = offset; - printf(" *** Profile Velocity at offset %d ***\n", offset); - break; - case 0x6087: // Torque Slope - offsets.torqueSlope = offset; - break; - case 0x6060: // Mode of Operation - offsets.modeOfOperation = offset; - break; - default: - printf(" *** Unknown entry 0x%04X:%02X ***\n", entry.index, entry.subindex); - break; - } - offset += bytes; - } - } - - // ========== 解析 TxPDO (Inputs) ========== - if (txPdo) { - uint16_t offset = 0; - printf("\nTxPDO 0x%04X mapping (%zu entries):\n", txPdo->index, txPdo->entries.size()); - - for (const auto& entry : txPdo->entries) { - uint8_t bytes = (entry.bitlen + 7) / 8; - printf(" Entry 0x%04X:%02X -> offset %d (%d bytes)\n", - entry.index, entry.subindex, offset, bytes); - - switch (entry.index) { - case 0x6041: // Status Word - offsets.statusWord = offset; - printf(" *** Status Word at offset %d ***\n", offset); - break; - case 0x6064: // Actual Position - offsets.actualPosition = offset; - break; - case 0x606C: // Actual Velocity - offsets.actualVelocity = offset; - break; - case 0x6077: // Actual Torque - offsets.actualTorque = offset; - break; - case 0x6061: // Mode of Operation Display - offsets.modeOfOperationDisplay = offset; - break; - case 0x603F: // Error Code - offsets.errorCode = offset; - break; - case 0x6078: // Actual Current - offsets.actualCurrent = offset; - printf(" *** Actual Current at offset %d ***\n", offset); - break; - default: - printf(" *** Unknown entry 0x%04X:%02X ***\n", entry.index, entry.subindex); - break; - } - offset += bytes; - } - } - - pdoOffsets[slaveId] = offsets; - - // ========== 打印摘要 ========== - printf("\n=== PDO Offset Summary ===\n"); - printf("RxPDO (Outputs):\n"); - printf(" Control Word: %d %s\n", offsets.controlWord, - offsets.controlWord == 0xFFFF ? "(INVALID)" : ""); - printf(" Target Position: %d %s\n", offsets.targetPosition, - offsets.targetPosition == 0xFFFF ? "(INVALID)" : ""); - printf(" Target Velocity: %d %s\n", offsets.targetVelocity, - offsets.targetVelocity == 0xFFFF ? "(INVALID)" : ""); - printf(" Target Torque: %d %s\n", offsets.targetTorque, - offsets.targetTorque == 0xFFFF ? "(INVALID)" : ""); - printf(" Profile Velocity: %d %s\n", offsets.profileVelocity, - offsets.profileVelocity == 0xFFFF ? "(NOT IN PDO)" : ""); - printf(" Profile Acc: %d %s\n", offsets.profileAcceleration, - offsets.profileAcceleration == 0xFFFF ? "(NOT IN PDO)" : ""); - printf(" Profile Dec: %d %s\n", offsets.profileDeceleration, - offsets.profileDeceleration == 0xFFFF ? "(NOT IN PDO)" : ""); - printf(" Mode of Operation: %d %s\n", offsets.modeOfOperation, - offsets.modeOfOperation == 0xFFFF ? "(INVALID)" : ""); - - printf("\nTxPDO (Inputs):\n"); - printf(" Status Word: %d %s\n", offsets.statusWord, - offsets.statusWord == 0xFFFF ? "(INVALID)" : ""); - printf(" Actual Position: %d %s\n", offsets.actualPosition, - offsets.actualPosition == 0xFFFF ? "(INVALID)" : ""); - printf(" Actual Velocity: %d %s\n", offsets.actualVelocity, - offsets.actualVelocity == 0xFFFF ? "(INVALID)" : ""); - printf(" Actual Torque: %d %s\n", offsets.actualTorque, - offsets.actualTorque == 0xFFFF ? "(INVALID)" : ""); - printf(" Mode Display: %d %s\n", offsets.modeOfOperationDisplay, - offsets.modeOfOperationDisplay == 0xFFFF ? "(INVALID)" : ""); - printf(" Error Code: %d %s\n", offsets.errorCode, - offsets.errorCode == 0xFFFF ? "(NOT IN PDO)" : ""); - printf(" Actual Current: %d %s\n", offsets.actualCurrent, - offsets.actualCurrent == 0xFFFF ? "(NOT IN PDO)" : ""); - printf("=================================\n"); - - if (offsets.controlWord == 0xFFFF || offsets.statusWord == 0xFFFF) { - printf("ERROR: Critical PDO offsets not configured!\n"); - return -1; - } - - return 0; -} - -// ==================== PDO 访问接口 ==================== - -void EcMaster::setControlWord(uint16_t slaveId, uint16_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].controlWord, value); - } else { - writeSdo(slaveId, 0x6040, 0, &value, EcDataType::Uint16, 100); - } -} - -uint16_t EcMaster::getStatusWord(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].statusWord); - } - uint16_t value; - readSdo(slaveId, 0x6041, 0, &value, EcDataType::Uint16, 100); - return value; -} - -void EcMaster::setTargetPosition(uint16_t slaveId, int32_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].targetPosition, value); - } else { - writeSdo(slaveId, 0x607A, 0, &value, EcDataType::Int32, 100); - } -} - -int32_t EcMaster::getActualPosition(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].actualPosition); - } - int32_t value; - readSdo(slaveId, 0x6064, 0, &value, EcDataType::Int32, 100); - return value; -} - -void EcMaster::setTargetVelocity(uint16_t slaveId, int32_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].targetVelocity, value); - } else { - writeSdo(slaveId, 0x60FF, 0, &value, EcDataType::Int32, 100); - } -} - -int32_t EcMaster::getActualVelocity(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].actualVelocity); - } - int32_t value; - readSdo(slaveId, 0x606C, 0, &value, EcDataType::Int32, 100); - return value; -} - -void EcMaster::setTargetTorque(uint16_t slaveId, int16_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].targetTorque, value); - } else { - writeSdo(slaveId, 0x6071, 0, &value, EcDataType::Int16, 100); - } -} - -int16_t EcMaster::getActualTorque(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].actualTorque); - } - int16_t value; - readSdo(slaveId, 0x6077, 0, &value, EcDataType::Int16, 100); - return value; -} - -void EcMaster::setModeOfOperation(uint16_t slaveId, int8_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].modeOfOperation, value); - } else { - writeSdo(slaveId, 0x6060, 0, &value, EcDataType::Int8, 100); - } -} - -int8_t EcMaster::getModeOfOperationDisplay(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].modeOfOperationDisplay); - } - int8_t value; - readSdo(slaveId, 0x6061, 0, &value, EcDataType::Int8, 100); - return value; -} - -void EcMaster::setProfileVelocity(uint16_t slaveId, uint32_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].profileVelocity, value); - } -} - -void EcMaster::setProfileAcceleration(uint16_t slaveId, uint32_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].profileAcceleration, value); - } -} - -void EcMaster::setProfileDeceleration(uint16_t slaveId, uint32_t value) { - if (inOp) { - writePdo(slaveId, pdoOffsets[slaveId].profileDeceleration, value); - } -} - -int16_t EcMaster::getCurrentValue(uint16_t slaveId) { - if (inOp) { - return readPdo(slaveId, pdoOffsets[slaveId].actualCurrent); - } - int16_t value; - readSdo(slaveId, 0x6078, 0, &value, EcDataType::Int16, 100); - return value; -} - -uint16_t EcMaster::getErrorCode(uint16_t slaveId) { - if (inOp) { - auto it = pdoOffsets.find(slaveId); - if (it != pdoOffsets.end() && it->second.errorCode != 0xFFFF) { - return readPdo(slaveId, it->second.errorCode); - } - } - uint16_t value = 0; - readSdo(slaveId, 0x603F, 0, &value, EcDataType::Uint16, 100); - return value; -} - -// ==================== 电机控制高级接口 ==================== - -int EcMaster::enable(uint16_t slaveId) { - if (inOp) { - setControlWord(slaveId, 0x0006); - osal_usleep(10000); - setControlWord(slaveId, 0x0007); - osal_usleep(10000); - setControlWord(slaveId, 0x000F); - } else { - uint16_t val = 0x0006; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - osal_usleep(10000); - val = 0x0007; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - osal_usleep(10000); - val = 0x000F; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - } - osal_usleep(10000); - return 0; -} - -int EcMaster::disable(uint16_t slaveId) { - if (inOp) { - setControlWord(slaveId, 0x0007); - osal_usleep(10000); - setControlWord(slaveId, 0x0000); - } else { - uint16_t val = 0x0007; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - osal_usleep(10000); - val = 0x0000; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - } - return 0; -} - -int EcMaster::faultReset(uint16_t slaveId) { - if (inOp) { - setControlWord(slaveId, 0x0080); - osal_usleep(10000); - setControlWord(slaveId, 0x0000); - } else { - uint16_t val = 0x0080; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - osal_usleep(10000); - val = 0x0000; - writeSdo(slaveId, 0x6040, 0, &val, EcDataType::Uint16, 100); - } - return 0; -} - -// ==================== SDO 读写 ==================== - -int EcMaster::readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs) { - int size = 0; - switch (type) { - case EcDataType::Boolean: - case EcDataType::Int8: - case EcDataType::Uint8: - size = 1; break; - case EcDataType::Int16: - case EcDataType::Uint16: - size = 2; break; - case EcDataType::Int32: - case EcDataType::Uint32: - case EcDataType::Real32: - size = 4; break; - case EcDataType::Real64: - size = 8; break; - default: return -1; - } - int wkc = ecx_SDOread(&ctx, slaveId, index, subindex, FALSE, &size, value, timeoutMs); - return wkc > 0 ? 0 : -1; -} - -int EcMaster::writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, void* value, EcDataType type, int timeoutMs) { - int size = 0; - switch (type) { - case EcDataType::Boolean: - case EcDataType::Int8: - case EcDataType::Uint8: - size = 1; break; - case EcDataType::Int16: - case EcDataType::Uint16: - size = 2; break; - case EcDataType::Int32: - case EcDataType::Uint32: - case EcDataType::Real32: - size = 4; break; - case EcDataType::Real64: - size = 8; break; - default: return -1; - } - int wkc = ecx_SDOwrite(&ctx, slaveId, index, subindex, FALSE, size, value, timeoutMs); - return wkc > 0 ? 0 : -1; -} - -// ==================== SM/FMMU 配置校验 ==================== - -void EcMaster::verifySmConfiguration(uint16_t slaveId) { - if (slaveId > (uint16_t)ctx.slavecount) { - printf("Invalid slave ID: %d\n", slaveId); - return; - } - - ec_slavet* slave = &ctx.slavelist[slaveId]; - - printf("\n========== SM Configuration for Slave %d ==========\n", slaveId); - - printf("PDO Mapping Sizes:\n"); - printf(" Outputs (Obytes): %d bytes\n", slave->Obytes); - printf(" Inputs (Ibytes): %d bytes\n", slave->Ibytes); - - printf("\nSync Manager Configuration:\n"); - const char* sm_names[] = {"MBoxOut", "MBoxIn", "Outputs", "Inputs"}; - for (int sm = 0; sm < 4; sm++) { - printf(" SM%d (%s): StartAddr=0x%04X, Length=%d bytes\n", - sm, sm_names[sm], - slave->SM[sm].StartAddr, - slave->SM[sm].SMlength); - } - - printf("\nFMMU Configuration:\n"); - printf(" Outputs mapped to IOmap at offset: %td\n", - (uint8_t*)slave->outputs - IOmap); - printf(" Inputs mapped to IOmap at offset: %td\n", - (uint8_t*)slave->inputs - IOmap); - - // 打印影子缓冲区信息 - auto it = shadowBuffers.find(slaveId); - if (it != shadowBuffers.end()) { - printf("\nShadow Buffer:\n"); - printf(" Size: %zu bytes\n", it->second.size); - printf(" Dirty: %s\n", it->second.dirty ? "true" : "false"); - } else { - printf("\nShadow Buffer: Not allocated\n"); - } - - printf("==================================================\n"); -} - -void EcMaster::verifyAllSlavesSmConfiguration() { - for (int i = 1; i <= ctx.slavecount; i++) { - verifySmConfiguration((uint16_t)i); - } -} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h deleted file mode 100644 index e4b67ad0..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_master.h +++ /dev/null @@ -1,328 +0,0 @@ -// ec_master.h -#ifndef EC_MASTER_H -#define EC_MASTER_H - -#include "ec_types.h" -#include -#include -#include -#include - -#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 - int readSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) { - EcDataType type; - if constexpr (std::is_same_v) type = EcDataType::Int8; - else if constexpr (std::is_same_v) type = EcDataType::Uint8; - else if constexpr (std::is_same_v) type = EcDataType::Int16; - else if constexpr (std::is_same_v) type = EcDataType::Uint16; - else if constexpr (std::is_same_v) type = EcDataType::Int32; - else if constexpr (std::is_same_v) type = EcDataType::Uint32; - else if constexpr (std::is_same_v) type = EcDataType::Real32; - else if constexpr (std::is_same_v) type = EcDataType::Real64; - else return -1; - return readSdo(slaveId, index, subindex, value, type, timeoutMs); - } - - template - int writeSdo(uint16_t slaveId, uint16_t index, uint8_t subindex, T* value, int timeoutMs = 100) { - EcDataType type; - if constexpr (std::is_same_v) type = EcDataType::Int8; - else if constexpr (std::is_same_v) type = EcDataType::Uint8; - else if constexpr (std::is_same_v) type = EcDataType::Int16; - else if constexpr (std::is_same_v) type = EcDataType::Uint16; - else if constexpr (std::is_same_v) type = EcDataType::Int32; - else if constexpr (std::is_same_v) type = EcDataType::Uint32; - else if constexpr (std::is_same_v) type = EcDataType::Real32; - else if constexpr (std::is_same_v) 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 shadowBuffers; - std::atomic anyPdoDirty{false}; // 是否有任何影子缓冲区有更新 - - int slaveCount = 0; - int expectedWKC = 0; - int currentgroup = 0; - - // 状态标志 - std::atomic inOp{false}; - std::atomic isRunning{false}; - std::atomic mappingdone{0}; - std::atomic dorun{0}; - std::atomic cyclicWkcErrorCount{0}; - - int64_t cycleTimeNs = 1000000; - - OSAL_THREAD_HANDLE cyclicThread; - OSAL_THREAD_HANDLE checkThread; - - // PDO 数据指针映射 - std::map inputs; - std::map outputs; - std::map inputSizes; - std::map outputSizes; - std::map outputOffsets; - - // PDO 偏移映射 - std::map pdoOffsets; - - // 辅助函数 - template - void writePdo(uint16_t slaveId, uint16_t offset, T value); - - template - 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 -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 -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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h deleted file mode 100644 index 7f8a919d..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_pdo_config.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h deleted file mode 100644 index 174f2460..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_types.h +++ /dev/null @@ -1,111 +0,0 @@ -// ec_types.h - 公共类型定义 -#ifndef EC_TYPES_H -#define EC_TYPES_H - -#include -#include -#include - -// 数据类型枚举 -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 entries; - std::string name; - bool isRxPdo = false; - - void updateMapCount() { - mapCount = entries.size(); - } -}; - -// 模块信息 -struct EcModule { - uint32_t moduleIdent = 0; - std::string name; - std::vector rxPdos; - std::vector txPdos; -}; - -// 从站配置信息 -struct EcSlaveConfig { - uint16_t slaveId = 0; - uint32_t productCode = 0; - uint32_t revisionNo = 0; - std::string name; - std::vector modules; - std::vector availableRxPdos; - std::vector 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 \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp deleted file mode 100644 index c721e3fe..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.cpp +++ /dev/null @@ -1,317 +0,0 @@ -// ec_xml_parser.cpp -#include "ec_xml_parser.h" -#include "rapidxml/xml_parser.h" -#include -#include -#include - -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 slaveConfigs; - std::unique_ptr xmlDoc; - - bool loadXmlFile(const std::string& path) { - try { - xmlDoc = std::make_unique(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 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 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()) {} -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 EcXmlParser::getAvailableRxPdos(uint16_t slaveId) const { - auto it = pImpl->slaveConfigs.find(slaveId); - return (it != pImpl->slaveConfigs.end()) ? it->second.availableRxPdos : std::vector(); -} - -std::vector EcXmlParser::getAvailableTxPdos(uint16_t slaveId) const { - auto it = pImpl->slaveConfigs.find(slaveId); - return (it != pImpl->slaveConfigs.end()) ? it->second.availableTxPdos : std::vector(); -} - -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(); -} \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h b/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h deleted file mode 100644 index c7b72536..00000000 --- a/cmvr-es/devices/motor/eyou_motor/ethercat/protocol/ec_xml_parser.h +++ /dev/null @@ -1,41 +0,0 @@ -// ec_xml_parser.h -#ifndef EC_XML_PARSER_H -#define EC_XML_PARSER_H - -#include "ec_types.h" -#include -#include -#include - -// 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 getAvailableRxPdos(uint16_t slaveId) const; - std::vector 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 pImpl; -}; - -#endif // EC_XML_PARSER_H \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp b/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp deleted file mode 100644 index d9e2f043..00000000 --- a/cmvr-es/devices/motor/eyou_motor/eyou_motor.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by cmvr on 2026/3/30. -// - -#include "eyou_motor.h" \ No newline at end of file diff --git a/cmvr-es/devices/motor/eyou_motor/eyou_motor.h b/cmvr-es/devices/motor/eyou_motor/eyou_motor.h deleted file mode 100644 index da4d8ae3..00000000 --- a/cmvr-es/devices/motor/eyou_motor/eyou_motor.h +++ /dev/null @@ -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(protocol_); - ethercat_protocol->initNode(node_id_); - ethercat_protocol->setQdd(node_id_,1); - } - } - }; - - - } -} diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 6b49aa06..634a715c 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,8 +15,7 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - ETHERCAT = 2, - CUSTOM = 3 + CUSTOM = 2 }; virtual ~MotorProtocolInterface() = default; diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h index 65f44ddc..cfc9bde9 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h @@ -1,12 +1,11 @@ // // Created by lgv on 2025/8/1. // -#pragma once #include "../abstract_motor.h" #include "motor/motor_protocol_interface.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" -#include "cmvr/config/robot_config/robot_config.pb.h" + /** * class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成 */ @@ -23,15 +22,6 @@ namespace cmvr { node_id_ = info_.id; } - Ti5Motor(const cmvr::config::MotorConfig& config) : AbstractMotor() { - info_.id = config.id(); - info_.joint_name = config.jointname(); - info_.limitQ = config.limitqlb(); - info_.limitQd = config.limitqd(); - node_id_ = info_.id; - } - - /** * 电机初始化在具体的类中完成 */ diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 91daf0ae..5759c3af 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -41,7 +41,6 @@ namespace cmvr::device{ class AbstractRobot: public AbstractDevice { public: - AbstractRobot() = default; explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {} ~AbstractRobot() override=default; diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 9cd2d11d..7770997b 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -35,8 +35,6 @@ #include "planner/joint_space_planner/include/joint_space_planner_creator.h" #include "planner/joint_space_planner/include/joint_space_planner.h" #include "ik_solver/include/pinocchio_dls_ik_solver.h" -#include "cmvr/config/robot_config/robot_config.pb.h" - namespace cmvr::device{ @@ -51,10 +49,12 @@ namespace cmvr::device{ ROBOT_ERROR } RobotStateMachine; +// typedef enum { +// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE +// } RobotRunningMode; + public: explicit HumanoidRobot(const XmlNode& cfg); - HumanoidRobot(const cmvr::config::HumanRobotConfig& config); - HumanoidRobot(const cmvr::config::EthercatRobotConfig& config); ~HumanoidRobot(); void init() override; @@ -232,7 +232,6 @@ namespace cmvr::device{ std::atomic is_head_busy_{false}, is_waist_busy_{false}; - std::string urdf_path_ = ""; }; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 9cc52666..0146c053 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -49,87 +49,6 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { } } - -template -HumanoidRobot::HumanoidRobot(const cmvr::config::HumanRobotConfig& config) { - try { - id_ = config.id(); - urdf_path_ = config.urdf(); - dof_ = DOF; - if (!pathExists(config.urdf())) { - throw runtime_error("urdf file does not exist"); - } - auto rcfg = cmvr::dyn::LoadRobotFromURDF( - config.urdf(), config.baselink()); - m_robot_ = std::make_shared >(rcfg); - - // 解析jointNames字符串，按逗号分隔 - joint_names_ = splitString(config.jointnames(), ","); - // 解析linkNames字符串，按逗号分隔 - link_names_ = splitString(config.linknames(), ","); - - if (joint_names_.size() != dof_) { - throw runtime_error("joint names size mismatched with dof"); - } - - m_state_ = m_robot_->MakeState(link_names_, joint_names_); - m_cctrl_ = make_shared >(m_robot_); - upd_freq_ = config.updfreq(); - CSP_buffer_ = make_shared >(config.buffersize()); - CSV_buffer_ = make_shared >(config.buffersize()); - CSC_buffer_ = make_shared >(config.buffersize()); - - upd_freq_ = 1000; - upd_timer_ = make_shared(); - upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), - [this] { update_state_(); }); - rsm_.store(ROBOT_READY); - } catch (exception &e) { - LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; - throw runtime_error(e.what()); - } -} - -template -HumanoidRobot::HumanoidRobot(const cmvr::config::EthercatRobotConfig& config) { - try { - id_ = config.id(); - urdf_path_ = config.urdf(); - dof_ = DOF; - if (!pathExists(config.urdf())) { - throw runtime_error("urdf file does not exist"); - } - auto rcfg = cmvr::dyn::LoadRobotFromURDF( - config.urdf(), config.baselink()); - m_robot_ = std::make_shared >(rcfg); - - // 解析jointNames字符串，按逗号分隔 - joint_names_ = splitString(config.jointnames(), ","); - // 解析linkNames字符串，按逗号分隔 - link_names_ = splitString(config.linknames(), ","); - - if (joint_names_.size() != dof_) { - throw runtime_error("joint names size mismatched with dof"); - } - - m_state_ = m_robot_->MakeState(link_names_, joint_names_); - m_cctrl_ = make_shared >(m_robot_); - upd_freq_ = config.updfreq(); - CSP_buffer_ = make_shared >(config.buffersize()); - CSV_buffer_ = make_shared >(config.buffersize()); - CSC_buffer_ = make_shared >(config.buffersize()); - - upd_freq_ = 1000; - upd_timer_ = make_shared(); - upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), - [this] { update_state_(); }); - rsm_.store(ROBOT_READY); - } catch (exception &e) { - LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; - throw runtime_error(e.what()); - } -} - template void HumanoidRobot::init() { motor_manager_ = MotorsInfo::getInstance()->getMotorManager(); @@ -140,7 +59,7 @@ void HumanoidRobot::init() { std::vector(7, 1.0)); - ik_solver_ = std::make_shared("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + ik_solver_ = std::make_shared("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", "PELVIS_S", "R_WRIST_R_S", "R_FINGER_TIP_FIXED"); diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index ed16d61d..d1b47e4e 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -990,21 +990,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps."; } - - -TEST(HumanoidRobotTest, EYouTest) { - auto& dmgr = DeviceManager::getInstance(); - auto robot_abs = dmgr.getDevice("ec_robot_01"); - auto robot = std::dynamic_pointer_cast>(robot_abs); - ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; - - std::vector init_cmd = { - {"L_SHOULDER_P", 0.1} - }; - // robot->moveJ(init_cmd, 0.1, 1.0); - - // robot->calibrateZeroQ("L_SHOULDER_P"); - - robot->torqueOff("L_SHOULDER_P"); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -} diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index c5531829..ab0f76d3 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -9,7 +9,6 @@ #include #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; diff --git a/cmvr-es/monitor/abstract_monitor.h b/cmvr-es/monitor/abstract_monitor.h index 244e5722..6b707e9c 100644 --- a/cmvr-es/monitor/abstract_monitor.h +++ b/cmvr-es/monitor/abstract_monitor.h @@ -13,7 +13,6 @@ namespace cmvr::monitor{ class AbstractMonitor { public: - AbstractMonitor() = default; explicit AbstractMonitor(const XmlNode &cfg){ cfg_ = cfg; freq_ = cfg_.getAttrDefault("freq", 1); diff --git a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h index a22256b3..89664362 100644 --- a/cmvr-es/monitor/diskmonitor/include/disk_monitor.h +++ b/cmvr-es/monitor/diskmonitor/include/disk_monitor.h @@ -8,21 +8,17 @@ #include #include "../../abstract_monitor.h" -#include "cmvr/config/monitor_config/monitor_config.pb.h" namespace cmvr::monitor { class DiskMonitor final : public AbstractMonitor{ public: explicit DiskMonitor(const XmlNode& config); - explicit DiskMonitor(const cmvr::config::DiskMonitorConfig& config); // 新增proto构造函数 ~DiskMonitor(); protected: bool check_() override; bool execution_() override; private: - void initFromProto(const cmvr::config::DiskMonitorConfig& config); // 新增初始化方法 - std::unordered_map folders_; std::vector dirsToClean_; // 需要清理的目录列表 mutable std::mutex mutex_; // 保护共享资源的互斥锁 @@ -31,4 +27,6 @@ namespace cmvr::monitor { }; } + + #endif //FOLDER_MONITOR_H \ No newline at end of file diff --git a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp index 9f4497cf..20630615 100644 --- a/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp +++ b/cmvr-es/monitor/diskmonitor/src/disk_monitor.cpp @@ -14,6 +14,7 @@ using namespace cmvr::monitor; namespace fs = std::filesystem; using namespace std::chrono; + // 文件信息结构体 struct FileInfo { std::string path; @@ -37,40 +38,51 @@ bool deleteOldestFile(const std::string& directoryPath) { struct dirent* entry; struct stat fileStat; + // 遍历目录中的每个条目 while ((entry = readdir(dir)) != nullptr) { + // 跳过 "." 和 ".." if (std::string(entry->d_name) == "." || std::string(entry->d_name) == "..") { continue; } + // 构建完整路径 std::string filePath = directoryPath + "/" + entry->d_name; + // 获取文件状态信息 if (stat(filePath.c_str(), &fileStat) == -1) { std::cerr << "Failed to get file status: " << filePath << std::endl; continue; } + // 只处理普通文件 if (S_ISREG(fileStat.st_mode)) { FileInfo fileInfo; fileInfo.path = filePath; + + // 优先使用 st_birthtime (如果可用)，否则使用 st_ctime #ifdef HAVE_STRUCT_STAT_ST_BIRTHTIME fileInfo.creationTime = fileStat.st_birthtime; #else fileInfo.creationTime = fileStat.st_ctime; #endif + files.push_back(fileInfo); } } closedir(dir); + // 如果没有找到文件，返回失败 if (files.empty()) { std::cout << "No files found in directory: " << directoryPath << std::endl; return false; } + // 按创建时间排序 std::sort(files.begin(), files.end(), compareByCreationTime); - std::string oldestFilePath = files[0].path; + // 删除最早创建的文件 + std::string oldestFilePath = files[0].path; if (unlink(oldestFilePath.c_str()) == -1) { std::cerr << "Failed to delete file: " << oldestFilePath << std::endl; return false; @@ -80,11 +92,11 @@ bool deleteOldestFile(const std::string& directoryPath) { return true; } + bool pathExists(const std::string& path) { struct stat buffer; return (stat(path.c_str(), &buffer) == 0); } - int countFilesInDirectory(const std::string& path) { DIR* dir = opendir(path.c_str()); if (!dir) { @@ -95,7 +107,9 @@ int countFilesInDirectory(const std::string& path) { int count = 0; struct dirent* entry; + // 遍历目录中的每个条目 while ((entry = readdir(dir)) != nullptr) { + // 跳过 "." 和 ".." if (std::string(entry->d_name) != "." && std::string(entry->d_name) != "..") { count++; } @@ -105,8 +119,8 @@ int countFilesInDirectory(const std::string& path) { return count; } -// XML构造函数 DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ + try { for (auto &node: config.getChildren()){ string fileDir = node.getAttrString("fileDir"); @@ -124,42 +138,13 @@ DiskMonitor::DiskMonitor(const XmlNode& config) : AbstractMonitor(config){ throw runtime_error("[DiskMonitor] (DiskMonitor): Failed "+ string(e.what())); } } - -// Proto构造函数 -DiskMonitor::DiskMonitor(const cmvr::config::DiskMonitorConfig& config) : AbstractMonitor() { - try { - initFromProto(config); - is_initialized_ = true; - LOG(INFO) << "[DiskMonitor]: Init DiskMonitor from proto Success"; - } - catch (const exception& e) { - LOG(ERROR) << "[DiskMonitor] (DiskMonitor): Failed " << e.what(); - throw runtime_error("[DiskMonitor] (DiskMonitor): Failed " + string(e.what())); - } -} - -// Proto初始化方法 -void DiskMonitor::initFromProto(const cmvr::config::DiskMonitorConfig& config) { - // 设置监控ID和频率（通过AbstractMonitor） - // 假设AbstractMonitor有setId和setFreq方法，如果没有需要添加 - // 这里根据你的AbstractMonitor实现来调整 - - for (const auto& folder : config.folders()) { - string fileDir = folder.filedir(); - if (!pathExists(fileDir)) { - throw runtime_error("[DiskMonitor] (initFromProto): path not exists: " + fileDir); - } - int maxVolume = folder.maxvolume(); - folders_[fileDir] = maxVolume; - } -} - DiskMonitor::~DiskMonitor() { folders_.clear(); } - bool DiskMonitor::check_() { + // 检查当前目录下的文件数量是否超过最大值 bool allOk = true; + // 遍历所有监控的文件夹 for (const auto& folder : folders_) { const std::string& path = folder.first; size_t maxFiles = folder.second; @@ -169,21 +154,28 @@ bool DiskMonitor::check_() { continue; } int nFileCount = countFilesInDirectory(path); - if (nFileCount > maxFiles) { + if (nFileCount > maxFiles) + { allOk = false; dirsToClean_.push_back(path); std::cout << "path need remove oldest file: " << path << std::endl; } + + } catch (const fs::filesystem_error& e) { std::cerr << "Filesystem error: " << e.what() << '\n'; } catch (const std::exception& e) { std::cerr << "Unexpected error: " << e.what() << '\n'; } } + return allOk; } + bool DiskMonitor::execution_() { + + // 删除创建时间最早的文件 std::lock_guard lock(mutex_); bool success = true; @@ -191,6 +183,6 @@ bool DiskMonitor::execution_() { deleteOldestFile(path); } - dirsToClean_.clear(); + dirsToClean_.clear(); // 清理完成后清空列表 return success; } \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_factory.h b/cmvr-es/monitor_manager/include/monitor_factory.h index cdd879a3..a0dbc900 100644 --- a/cmvr-es/monitor_manager/include/monitor_factory.h +++ b/cmvr-es/monitor_manager/include/monitor_factory.h @@ -5,8 +5,8 @@ #ifndef MONITOR_FACTORY_H #define MONITOR_FACTORY_H -#include "monitor/abstract_monitor.h" -#include "cmvr/config/monitor_config/monitor_config.pb.h" + +#include "../../monitor/abstract_monitor.h" namespace cmvr::monitor { @@ -14,17 +14,14 @@ namespace cmvr::monitor { public: MonitorFactory() = default; - - // 原有的XML方式 template std::shared_ptr create(const XmlNode& cfg); std::shared_ptr create(const XmlNode& cfg); - - // 新增proto方式 - template - std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); - std::shared_ptr create(const cmvr::config::DiskMonitorConfig& config); }; + + } + + #endif //MONITOR_FACTORY_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/include/monitor_manager.h b/cmvr-es/monitor_manager/include/monitor_manager.h index 422ece1f..8c53a401 100644 --- a/cmvr-es/monitor_manager/include/monitor_manager.h +++ b/cmvr-es/monitor_manager/include/monitor_manager.h @@ -15,38 +15,23 @@ namespace cmvr::monitor { public: MonitorManager(const MonitorManager&) = delete; MonitorManager& operator=(const MonitorManager&) = delete; - - // 原有的XML方式 static MonitorManager& getInstance(const XmlNode &cfg); - - // 新增proto方式 - static MonitorManager& getInstance(const cmvr::config::MonitorManagerConfig& config); - static MonitorManager& getInstance(); static void destroyInstance(); private: - // 原有的XML方式 - explicit MonitorManager(const XmlNode &cfg); - - // 新增proto方式 - explicit MonitorManager(const cmvr::config::MonitorManagerConfig& config); - - void init_monitors(); // XML方式 - void init_monitors_from_proto(); // Proto方式 - + explicit MonitorManager (const XmlNode &cfg); + void init_monitors(); private: static std::once_flag init_flag_; static std::shared_ptr instance_; - // 两种配置方式，只有一个有效 - bool use_proto_ = false; - XmlNode cfg_; // XML配置 - cmvr::config::MonitorManagerConfig proto_cfg_; // Proto配置 - + XmlNode cfg_; std::shared_ptr monitor_factory_; std::unordered_map> monitors_; }; } + + #endif //MONITOR_MANAGER_H \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_factory.cpp b/cmvr-es/monitor_manager/src/monitor_factory.cpp index e770f294..821373e2 100644 --- a/cmvr-es/monitor_manager/src/monitor_factory.cpp +++ b/cmvr-es/monitor_manager/src/monitor_factory.cpp @@ -3,12 +3,12 @@ // #include "../include/monitor_factory.h" + #include "../../monitor/diskmonitor/include/disk_monitor.h" using namespace std; using namespace cmvr::monitor; -// XML方式的模板特化 template std::shared_ptr MonitorFactory::create(const XmlNode&); template @@ -17,30 +17,8 @@ std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { return std::make_shared(cfg); } else { - LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; - throw runtime_error("[MonitorFactory]: Unsupported monitor type"); + LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; + throw runtime_error("[DeviceFactory]: Unsupported device type"); } } -// Proto方式的模板特化 -template std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig&); - -template -std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { - if constexpr (std::is_same_v) { - return std::make_shared(config); - } - else { - LOG(ERROR) << "[MonitorFactory]: Unsupported monitor type"; - throw runtime_error("[MonitorFactory]: Unsupported monitor type"); - } -} - -std::shared_ptr MonitorFactory::create(const XmlNode& cfg) { - // 保留原有实现 - return nullptr; -} - -std::shared_ptr MonitorFactory::create(const cmvr::config::DiskMonitorConfig& config) { - return std::make_shared(config); -} \ No newline at end of file diff --git a/cmvr-es/monitor_manager/src/monitor_manager.cpp b/cmvr-es/monitor_manager/src/monitor_manager.cpp index dde9b386..2dd9e2cf 100644 --- a/cmvr-es/monitor_manager/src/monitor_manager.cpp +++ b/cmvr-es/monitor_manager/src/monitor_manager.cpp @@ -3,7 +3,8 @@ // #include "../include/monitor_manager.h" -#include "monitor/diskmonitor/include/disk_monitor.h" + +#include "../../monitor/diskmonitor/include/disk_monitor.h" using namespace std; using namespace cmvr::monitor; @@ -11,19 +12,11 @@ using namespace cmvr::monitor; std::shared_ptr MonitorManager::instance_ = nullptr; std::once_flag MonitorManager::init_flag_; -// XML方式构造函数 -MonitorManager::MonitorManager(const XmlNode& cfg) : use_proto_(false), cfg_(cfg) { +MonitorManager::MonitorManager(const XmlNode& cfg) { monitor_factory_ = make_shared(); + cfg_ = cfg; init_monitors(); } - -// Proto方式构造函数 -MonitorManager::MonitorManager(const cmvr::config::MonitorManagerConfig& config) : use_proto_(true), proto_cfg_(config) { - monitor_factory_ = make_shared(); - init_monitors_from_proto(); -} - -// XML方式单例 MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { std::call_once(init_flag_, [&cfg] { instance_.reset(new MonitorManager(cfg)); @@ -31,17 +24,9 @@ MonitorManager& MonitorManager::getInstance(const XmlNode& cfg) { return *instance_; } -// Proto方式单例 -MonitorManager& MonitorManager::getInstance(const cmvr::config::MonitorManagerConfig& config) { - std::call_once(init_flag_, [&config] { - instance_.reset(new MonitorManager(config)); - }); - return *instance_; -} - MonitorManager& MonitorManager::getInstance() { if (!instance_) { - throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance() first."); + throw std::runtime_error("[MonitorManager] (getInstance): MonitorManager not initialized. Call getInstance(const XmlNode&) first."); } return *instance_; } @@ -50,14 +35,13 @@ void MonitorManager::destroyInstance() { instance_.reset(); } -// XML方式初始化 void MonitorManager::init_monitors() { - try { + try{ auto diskmonitor_node = cfg_.getChild("DiskMonitor"); auto id = diskmonitor_node.getAttrString("id"); auto monitor = monitor_factory_->create(diskmonitor_node); if (monitor == nullptr) { - throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id); + throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); } monitors_[id] = monitor; } @@ -65,31 +49,5 @@ void MonitorManager::init_monitors() { LOG(ERROR) << e.what(); throw runtime_error(e.what()); } -} -// Proto方式初始化 -void MonitorManager::init_monitors_from_proto() { - try { - if (!proto_cfg_.has_disk_monitor()) { - LOG(WARNING) << "[MonitorManager]: No disk_monitor config found"; - return; - } - - const auto& disk_monitor_cfg = proto_cfg_.disk_monitor(); - if (!disk_monitor_cfg.enable()) { - LOG(INFO) << "[MonitorManager]: Disk monitor is disabled"; - return; - } - - auto id = disk_monitor_cfg.id(); - auto monitor = monitor_factory_->create(disk_monitor_cfg); - if (monitor == nullptr) { - throw std::runtime_error("[MonitorManager]: Factory returned nullptr for monitor ID: " + id); - } - monitors_[id] = monitor; - } - catch (const exception& e) { - LOG(ERROR) << e.what(); - throw runtime_error(e.what()); - } } \ No newline at end of file diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 98ea4199..4ef50063 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -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}) diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h deleted file mode 100644 index 02253dda..00000000 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/cmvr-es/service/grpc/include/server_runner.h b/cmvr-es/service/grpc/include/server_runner.h index 7ad0706e..be8eadf8 100644 --- a/cmvr-es/service/grpc/include/server_runner.h +++ b/cmvr-es/service/grpc/include/server_runner.h @@ -11,7 +11,6 @@ #include #include #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 server_; std::string address_; diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp deleted file mode 100644 index a48e03b2..00000000 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ /dev/null @@ -1,1208 +0,0 @@ -// -// Created by xtkuang on 2025/6/1. -// -/** - * @file grpc_agv_service.cpp - * @brief gRPC AGV 服务实现。 - * 每个 RPC 方法从请求头中提取 device_id，通过 DeviceManager 获取对应的 AGV 设备， - * 调用设备层的具体方法，并将结果转换为 Protobuf 响应。 - * 统一处理异常，设置响应头中的 success/error_message 和时间戳。 - * @author xtkuang - * @date 2025-06-01 - */ - -#include "./service/grpc/include/grpc_agv_service.h" - -#include - -#include "devices/agv/agv_src1100/include/agv_src1100.h" - -#include "devices/agv/abstract_agv.h" - -namespace cmvr::service { - - /** - * @brief 构造函数，获取 DeviceManager 单例。 - */ - gRPCAGVServiceImpl::gRPCAGVServiceImpl() : dmgr_(device::DeviceManager::getInstance()) { - } - - // ===================== GetStatusInfo ===================== - /** - * @brief 获取 AGV 基本信息。 - * @details 从请求中提取 device_id，调用设备层的 getStatusInfo，填充响应。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回状态信息和响应头。 - * @return 始终返回 OK，错误通过 response.header 传递。 - */ - grpc::Status gRPCAGVServiceImpl::GetStatusInfo( - grpc::ServerContext* context, - const api::GetAgvStatusInfoCommand_Request* request, - api::GetAgvStatusInfoCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (GetStatusInfo): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - device::AbstractAgv::AgvStatusInfo device_status; - agv_device->getStatusInfo(device_status); - - // 映射到 Protobuf 消息 - auto* proto_status = response->mutable_status(); - proto_status->set_id(device_status.id); - proto_status->set_vehicle_id(device_status.vehicle_id); - proto_status->set_version(device_status.version); - proto_status->set_model(device_status.model); - proto_status->set_dsp_version(device_status.dsp_version); - proto_status->set_current_ip(device_status.current_ip); - proto_status->set_mac(device_status.mac); - proto_status->set_rssi(device_status.rssi); - proto_status->set_ret_code(device_status.ret_code); - proto_status->set_err_msg(device_status.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "GetStatusInfo device_id:" << dev_id << " vehicle_id: " << device_status.vehicle_id; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== GetBatteryStatus ===================== - /** - * @brief 查询电池状态。 - * @details 支持 simple 参数，调用设备层 getBatteryStatus。 - * @param context 未使用。 - * @param request 包含设备 ID 和 simple 标志。 - * @param response 返回电池信息。 - */ - grpc::Status gRPCAGVServiceImpl::GetBatteryStatus( - grpc::ServerContext* context, - const api::RobotStatusBatteryCommand_Request* request, - api::RobotStatusBatteryCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (GetBatteryStatus): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - bool simple = false; - if (request->has_data()) - { - simple = request->data().simple(); - } - - device::AbstractAgv::BatteryStatus bat_info; - agv_device->getBatteryStatus(bat_info, simple); - - auto* proto_bat = response->mutable_status(); - proto_bat->set_battery_level(bat_info.battery_level); - proto_bat->set_battery_temp(bat_info.battery_temp); - proto_bat->set_charging(bat_info.charging); - proto_bat->set_voltage(bat_info.voltage); - proto_bat->set_current(bat_info.current); - proto_bat->set_max_charge_voltage(bat_info.max_charge_voltage); - proto_bat->set_max_charge_current(bat_info.max_charge_current); - proto_bat->set_manual_charge(bat_info.manual_charge); - proto_bat->set_auto_charge(bat_info.auto_charge); - proto_bat->set_battery_cycle(bat_info.battery_cycle); - proto_bat->set_battery_user_data(bat_info.battery_user_data); - proto_bat->set_extra(bat_info.extra); - proto_bat->set_ret_code(bat_info.ret_code); - proto_bat->set_create_on(bat_info.create_on); - proto_bat->set_err_msg(bat_info.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "GetBatteryStatus device_id:" << dev_id << " level: " << bat_info.battery_level; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== GetRobotLocation ===================== - /** - * @brief 查询机器人当前位置。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回坐标、朝向、置信度等。 - */ - grpc::Status gRPCAGVServiceImpl::GetRobotLocation( - grpc::ServerContext* context, - const api::RobotStatusLocCommand_Request* request, - api::RobotStatusLocCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (GetRobotLocation): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - device::AbstractAgv::RobotLocation loc_info; - agv_device->getRobotLocation(loc_info); - - auto* proto_loc = response->mutable_status(); - proto_loc->set_x(loc_info.x); - proto_loc->set_y(loc_info.y); - proto_loc->set_angle(loc_info.angle); - proto_loc->set_confidence(loc_info.confidence); - proto_loc->set_current_station(loc_info.current_station); - proto_loc->set_last_station(loc_info.last_station); - proto_loc->set_loc_method(loc_info.loc_method); - proto_loc->set_ret_code(loc_info.ret_code); - proto_loc->set_create_on(loc_info.create_on); - proto_loc->set_err_msg(loc_info.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "GetRobotLocation device_id:" << dev_id << " x: " << loc_info.x << ", y: " << loc_info.y; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotConfigDownloadMap ===================== - /** - * @brief 下载指定地图。 - * @param context 未使用。 - * @param request 包含设备 ID 和地图名称。 - * @param response 返回地图 JSON 内容或错误码。 - */ - grpc::Status gRPCAGVServiceImpl::RobotConfigDownloadMap( - grpc::ServerContext* context, - const api::RobotConfigDownloadMapCommand_Request* request, - api::RobotConfigDownloadMapCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigDownloadMap): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - std::string map_name = request->data().map_name(); - device::AbstractAgv::DownloadMapResult down_info; - agv_device->downloadMap(down_info, map_name); - - auto* proto_res = response->mutable_status(); - proto_res->set_ret_code(down_info.ret_code); - proto_res->set_create_on(down_info.create_on); - proto_res->set_err_msg(down_info.err_msg); - proto_res->set_map_content(down_info.map_content); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "DownloadMap device_id:" << dev_id << " map: " << map_name << ", ret_code: " << down_info.ret_code; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== GetMapStatus ===================== - /** - * @brief 查询地图状态。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回当前地图、地图列表及文件详情。 - */ - grpc::Status gRPCAGVServiceImpl::GetMapStatus( - grpc::ServerContext* context, - const api::RobotStatusMapCommand_Request* request, - api::RobotStatusMapCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (GetMapStatus): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - device::AbstractAgv::MapStatus map_info; - agv_device->getMapStatus(map_info); - - auto* proto_map = response->mutable_status(); - proto_map->set_current_map(map_info.current_map); - proto_map->set_current_map_md5(map_info.current_map_md5); - proto_map->set_ret_code(map_info.ret_code); - proto_map->set_create_on(map_info.create_on); - proto_map->set_err_msg(map_info.err_msg); - - for (auto& name : map_info.maps) - { - proto_map->add_maps(name); - } - - for (auto& file : map_info.map_files_info) - { - auto* proto_file = proto_map->add_map_files_info(); - proto_file->set_name(file.name); - proto_file->set_modified(file.modified); - proto_file->set_size(file.size); - } - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "GetMapStatus device_id:" << dev_id << " current map: " << map_info.current_map; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotConfigUploadMap ===================== - /** - * @brief 上传地图。 - * @param context 未使用。 - * @param request 包含设备 ID 和地图 JSON 字符串。 - * @param response 返回上传结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotConfigUploadMap( - grpc::ServerContext* context, - const api::RobotConfigUploadMapCommand_Request* request, - api::RobotConfigUploadMapCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigUploadMap): id=" << dev_id; - const auto agv_device = dmgr_.getDevice(dev_id); - - std::string map_json = request->data().map_content(); - device::AbstractAgv::UploadMapResult upload_info; - agv_device->uploadMap(upload_info, map_json); - - auto* proto_res = response->mutable_status(); - proto_res->set_ret_code(upload_info.ret_code); - proto_res->set_create_on(upload_info.create_on); - proto_res->set_err_msg(upload_info.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "UploadMap device_id:" << dev_id << " ret_code: " << upload_info.ret_code; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotConfigLock ===================== - /** - * @brief 抢占机器人控制权。 - * @param context 未使用。 - * @param request 包含设备 ID 和抢占者昵称。 - * @param response 返回抢占结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotConfigLock( - grpc::ServerContext* context, - const api::RobotConfigLockCommand_Request* request, - api::RobotConfigLockCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigLock): id=" << dev_id; - - const auto agv_device = dmgr_.getDevice(dev_id); - - std::string nick_name = request->data().nick_name(); - device::AbstractAgv::LockResult lock_res; - agv_device->lockRobotControl(lock_res, nick_name); - - auto* proto_status = response->mutable_status(); - proto_status->set_ret_code(lock_res.ret_code); - proto_status->set_create_on(lock_res.create_on); - proto_status->set_err_msg(lock_res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "Lock robot control, device_id:" << dev_id << ", nick:" << nick_name << ", ret_code:" << lock_res.ret_code; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== GetCurrentLockStatus ===================== - /** - * @brief 查询当前控制权持有者。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回锁定状态、持有者信息等。 - */ - grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( - grpc::ServerContext* context, - const api::RobotStatusCurrentLockCommand_Request* request, - api::RobotStatusCurrentLockCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (GetCurrentLockStatus): id=" << dev_id; - - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::CurrentLockStatus lock_info; - agv_dev->getCurrentLockStatus(lock_info); - - auto* proto_status = response->mutable_status(); - proto_status->set_locked(lock_info.locked); - proto_status->set_ip(lock_info.ip); - proto_status->set_port(lock_info.port); - proto_status->set_type(lock_info.type); - proto_status->set_nick_name(lock_info.nick_name); - proto_status->set_time_t(lock_info.time_t); - proto_status->set_desc(lock_info.desc); - proto_status->set_ret_code(lock_info.ret_code); - proto_status->set_create_on(lock_info.create_on); - proto_status->set_err_msg(lock_info.err_msg); - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - - LOG(INFO) << "Query current lock status, device_id:" << dev_id - << ", locked:" << lock_info.locked << ", nick:" << lock_info.nick_name; - return grpc::Status::OK; - } - catch (std::exception &e) - { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotMotionControl ===================== - /** - * @brief 开环速度运动控制。 - * @param context 未使用。 - * @param request 包含设备 ID 和速度参数。 - * @param response 返回指令下发结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotMotionControl( - grpc::ServerContext* context, - const api::RobotMotionControlCommand_Request* request, - api::RobotMotionControlCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotMotionControl): target device_id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::MotionCtrlReq req; - auto& input_data = request->data(); - req.vx = input_data.vx(); - req.vy = input_data.vy(); - req.w = input_data.w(); - req.steer = input_data.steer(); - req.real_steer = input_data.real_steer(); - req.duration = input_data.duration(); - - device::AbstractAgv::MotionCtrlRes res; - agv_dev->robotMotionControl(res, req); - - auto* resp_status = response->mutable_status(); - resp_status->set_ret_code(res.ret_code); - resp_status->set_create_on(res.create_on); - resp_status->set_err_msg(res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "RobotMotionControl finish, dev_id:" << dev_id - << " vx=" << req.vx << " vy=" << req.vy << " w=" << req.w - << " ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotLoadMap ===================== - /** - * @brief 切换载入地图。 - * @param context 未使用。 - * @param request 包含设备 ID 和目标地图名称。 - * @param response 返回切换结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotLoadMap( - grpc::ServerContext* context, - const api::RobotLoadMapCommand_Request* request, - api::RobotLoadMapCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotLoadMap): target device_id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::LoadMapReq req; - auto& input_data = request->data(); - req.map_name = input_data.map_name(); - - device::AbstractAgv::LoadMapRes res; - agv_dev->robotLoadMap(res, req); - - auto* resp_status = response->mutable_status(); - resp_status->set_ret_code(res.ret_code); - resp_status->set_create_on(res.create_on); - resp_status->set_err_msg(res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "RobotLoadMap finish, dev_id:" << dev_id - << " map_name=" << req.map_name - << " ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== QueryLoadMapStatus ===================== - /** - * @brief 查询地图载入状态。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回 loadmap_status。 - */ - grpc::Status gRPCAGVServiceImpl::QueryLoadMapStatus( - grpc::ServerContext* context, - const api::RobotQueryLoadMapStatusCommand_Request* request, - api::RobotQueryLoadMapStatusCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (QueryLoadMapStatus): device_id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::QueryLoadMapStatusRes res; - agv_dev->queryLoadMapStatus(res); - - auto* resp_status = response->mutable_status(); - resp_status->set_loadmap_status(res.loadmap_status); - resp_status->set_ret_code(res.ret_code); - resp_status->set_create_on(res.create_on); - resp_status->set_err_msg(res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "QueryLoadMapStatus finish dev_id:" << dev_id - << " loadmap_status=" << res.loadmap_status - << " ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== QueryStationList ===================== - /** - * @brief 查询当前地图所有站点信息。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回站点列表。 - */ - grpc::Status gRPCAGVServiceImpl::QueryStationList( - grpc::ServerContext* context, - const api::QueryStationListCommand_Request* request, - api::QueryStationListCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (QueryStationList): device_id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::QueryStationRes res; - agv_dev->queryStationList(res); - - auto* resp_status = response->mutable_status(); - resp_status->set_ret_code(res.ret_code); - resp_status->set_create_on(res.create_on); - resp_status->set_err_msg(res.err_msg); - - for (auto& st : res.stations) - { - auto* pb_st = resp_status->add_stations(); - pb_st->set_id(st.id); - pb_st->set_type(st.type); - pb_st->set_x(st.x); - pb_st->set_y(st.y); - pb_st->set_r(st.r); - pb_st->set_desc(st.desc); - pb_st->set_executor(st.executor); - pb_st->set_prepoint(st.prepoint); - pb_st->set_recfile(st.recfile); - pb_st->set_spin(st.spin); - pb_st->set_use_down_pgv(st.use_down_pgv); - } - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "QueryStationList finish dev_id:" << dev_id - << " station_count=" << res.stations.size() - << " ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotGoTargetList ===================== - /** - * @brief 指定路径导航（多点）。 - * @param context 未使用。 - * @param request 包含设备 ID 和 move_task_list。 - * @param response 返回下发结果（仅表示指令被接收）。 - */ - grpc::Status gRPCAGVServiceImpl::RobotGoTargetList( - grpc::ServerContext* context, - const api::RobotGoTargetListCommand_Request* request, - api::RobotGoTargetListCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotGoTargetList): target device_id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::GoTargetListReq req; - const auto& pb_list = request->data().move_task_list(); - for (const auto& pb_item : pb_list) - { - device::AbstractAgv::MoveTaskItem task; - task.task_id = pb_item.task_id(); - task.source_id = pb_item.source_id(); - task.id = pb_item.id(); - task.operation = pb_item.operation(); - task.jack_height = pb_item.jack_height(); - req.move_task_list.push_back(task); - } - - device::AbstractAgv::GoTargetListRes res; - agv_dev->robotGoTargetList(res, req); - - auto* resp_status = response->mutable_status(); - resp_status->set_ret_code(res.ret_code); - resp_status->set_create_on(res.create_on); - resp_status->set_err_msg(res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "RobotGoTargetList finish dev_id:" << dev_id - << " task_count=" << req.move_task_list.size() - << " ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotStatusTaskCurrent ===================== - /** - * @brief 查询当前实时导航状态。 - * @param context 未使用。 - * @param request 包含设备 ID 和 simple 标志。 - * @param response 返回任务状态、类型、目标、路径等。 - */ - grpc::Status gRPCAGVServiceImpl::RobotStatusTaskCurrent( - grpc::ServerContext* context, - const api::RobotStatusTaskCurrentCommand_Request* request, - api::RobotStatusTaskCurrentCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTaskCurrent): id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::RobotStatusTaskCurrentReq req; - if (request->has_data()) - { - req.simple = request->data().simple(); - } - - device::AbstractAgv::RobotStatusTaskCurrentRes res; - agv_dev->robotStatusTaskCurrent(res, req); - - auto* out_data = response->mutable_data(); - out_data->set_ret_code(res.ret_code); - out_data->set_create_on(res.create_on); - out_data->set_err_msg(res.err_msg); - - out_data->set_task_status(res.task_status); - out_data->set_task_type(res.task_type); - out_data->set_target_id(res.target_id); - out_data->set_move_status_info(res.move_status_info); - - for (double p : res.target_point) - out_data->add_target_point(p); - for (auto& s : res.finished_path) - out_data->add_finished_path(s); - for (auto& s : res.unfinished_path) - out_data->add_unfinished_path(s); - - for (auto& c : res.containers) - { - auto* pc = out_data->add_containers(); - pc->set_container_name(c.container_name); - pc->set_desc(c.desc); - pc->set_goods_id(c.goods_id); - pc->set_has_goods(c.has_goods); - } - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "RobotStatusTaskCurrent dev_id:" << dev_id << " task_status:" << res.task_status; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotStatusTaskPackage ===================== - /** - * @brief 批量查询任务状态。 - * @param context 未使用。 - * @param request 包含设备 ID 和 task_ids 列表。 - * @param response 返回任务状态包（进度、距离、每个任务状态等）。 - */ - grpc::Status gRPCAGVServiceImpl::RobotStatusTaskPackage( - grpc::ServerContext* context, - const api::RobotStatusTaskPackageCommand_Request* request, - api::RobotStatusTaskPackageCommand_Feedback* response) - { - try { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTaskPackage): id=" << dev_id; - const auto agv_dev = dmgr_.getDevice(dev_id); - - device::AbstractAgv::QueryTaskStatusPackageReq req; - const auto& pb_ids = request->data().task_ids(); - for (auto& tid : pb_ids) - { - req.task_ids.push_back(tid); - } - - device::AbstractAgv::QueryTaskStatusPackageRes res; - agv_dev->robotStatusTaskPackage(res, req); - - auto* out_data = response->mutable_data(); - out_data->set_ret_code(res.ret_code); - out_data->set_create_on(res.create_on); - out_data->set_err_msg(res.err_msg); - - auto* pkg_pb = out_data->mutable_task_status_package(); - auto& pkg_data = res.task_status_package; - pkg_pb->set_closest_target(pkg_data.closest_target); - pkg_pb->set_source_name(pkg_data.source_name); - pkg_pb->set_target_name(pkg_data.target_name); - pkg_pb->set_percentage(pkg_data.percentage); - pkg_pb->set_distance(pkg_data.distance); - pkg_pb->set_info(pkg_data.info); - - for (auto& st_item : pkg_data.task_status_list) - { - auto* st_pb = pkg_pb->add_task_status_list(); - st_pb->set_task_id(st_item.task_id); - st_pb->set_status(st_item.status); - st_pb->set_type(st_item.type); - } - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "RobotStatusTaskPackage dev_id:" << dev_id << " query task count:" << req.task_ids.size() << " ret_code:" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception &e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotGoTarget ===================== - /** - * @brief 单点站点自动规划导航。 - * @param context 未使用。 - * @param request 包含设备 ID 和完整的导航参数（支持自由导航、动作、PGV、脚本等）。 - * @param response 返回下发结果。 - * @warning 严禁用于多车调度场景，仅限单车测试。 - */ - grpc::Status gRPCAGVServiceImpl::RobotGoTarget( - grpc::ServerContext* context, - const api::RobotGoTargetCommand_Request* request, - api::RobotGoTargetCommand_Feedback* response) - { - try - { - std::string dev_id = request->header().device_id(); - LOG(INFO) << "[RobotGoTarget] device_id:" << dev_id; - auto agv_dev = dmgr_.getDevice(dev_id); - if (!agv_dev) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message("device not found"); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - - device::AbstractAgv::RobotGoTargetReq req; - auto& pb_data = request->data(); - - // -------- 必填字段 -------- - req.source_id = pb_data.source_id(); - req.id = pb_data.id(); - if (pb_data.has_task_id()) req.task_id = pb_data.task_id(); - - // -------- 运动参数 -------- - if (pb_data.has_angle()) req.angle = pb_data.angle(); - if (pb_data.has_method()) req.method = pb_data.method(); - if (pb_data.has_max_speed()) req.max_speed = pb_data.max_speed(); - if (pb_data.has_max_wspeed()) req.max_wspeed = pb_data.max_wspeed(); - if (pb_data.has_max_acc()) req.max_acc = pb_data.max_acc(); - if (pb_data.has_max_wacc()) req.max_wacc = pb_data.max_wacc(); - if (pb_data.has_duration()) req.duration = pb_data.duration(); - if (pb_data.has_orientation()) req.orientation = pb_data.orientation(); - if (pb_data.has_spin()) req.spin = pb_data.spin(); - if (pb_data.has_delay()) req.delay = pb_data.delay(); - if (pb_data.has_start_rot_dir()) req.start_rot_dir = pb_data.start_rot_dir(); - if (pb_data.has_end_rot_dir()) req.end_rot_dir = pb_data.end_rot_dir(); - if (pb_data.has_reach_dist()) req.reach_dist = pb_data.reach_dist(); - if (pb_data.has_reach_angle()) req.reach_angle = pb_data.reach_angle(); - if (pb_data.has_skill_name()) req.skill_name = pb_data.skill_name(); - - // -------- PGV 二次定位 -------- - if (pb_data.has_pgv()) - { - auto& pgv_pb = pb_data.pgv(); - req.pgv.use_pgv = pgv_pb.use_pgv(); - req.pgv.use_down_pgv = pgv_pb.use_down_pgv(); - if (pgv_pb.has_pgv_adjust_dist()) req.pgv.pgv_adjust_dist = pgv_pb.pgv_adjust_dist(); - if (pgv_pb.has_pgv_adjust_cx()) req.pgv.pgv_adjust_cx = pgv_pb.pgv_adjust_cx(); - if (pgv_pb.has_pgv_adjust_cy()) req.pgv.pgv_adjust_cy = pgv_pb.pgv_adjust_cy(); - if (pgv_pb.has_pgv_x_adjust()) req.pgv.pgv_x_adjust = pgv_pb.pgv_x_adjust(); - } - - // -------- 设备动作 -------- - if (pb_data.has_operation()) req.operation = pb_data.operation(); - if (pb_data.has_jack_height()) req.jack_height = pb_data.jack_height(); - if (pb_data.has_start_height()) req.start_height = pb_data.start_height(); - if (pb_data.has_end_height()) req.end_height = pb_data.end_height(); - if (pb_data.has_fork_mid_height()) req.fork_mid_height = pb_data.fork_mid_height(); - if (pb_data.has_fork_dist()) req.fork_dist = pb_data.fork_dist(); - if (pb_data.has_direction()) req.direction = pb_data.direction(); - if (pb_data.has_recognize()) req.recognize = pb_data.recognize(); - if (pb_data.has_recfile()) req.recfile = pb_data.recfile(); - - // -------- 音频 -------- - if (pb_data.has_sounds_args()) - { - auto& s_pb = pb_data.sounds_args(); - if (s_pb.has_name()) req.sounds_args.name = s_pb.name(); - if (s_pb.has_loop()) req.sounds_args.loop = s_pb.loop(); - if (s_pb.has_stop()) req.sounds_args.stop = s_pb.stop(); - } - - // -------- WaitDI -------- - if (pb_data.has_wait_di_args()) - { - auto& di_pb = pb_data.wait_di_args(); - req.wait_di_args.timeout = di_pb.timeout(); - for (auto& item : di_pb.di()) - { - device::AbstractAgv::DIItem di; - di.id = item.id(); - di.status = item.status(); - req.wait_di_args.DI.push_back(di); - } - } - - // -------- SetDO -------- - if (pb_data.has_set_do_args()) - { - auto& do_pb = pb_data.set_do_args(); - for (auto& item : do_pb.do_list()) - { - device::AbstractAgv::DOItem d; - d.id = item.id(); - d.status = item.status(); - req.set_do_args.DO.push_back(d); - } - } - - // -------- 脚本 -------- - if (pb_data.has_script_name()) req.script_name = pb_data.script_name(); - if (pb_data.has_script_stage()) req.script_stage = pb_data.script_stage(); - if (pb_data.has_script_args()) - { - auto& s_arg_pb = pb_data.script_args(); - for (auto& kv : s_arg_pb.str_kv()) req.script_args.str_kv[kv.first] = kv.second; - for (auto& kv : s_arg_pb.num_kv()) req.script_args.num_kv[kv.first] = kv.second; - for (auto& d : s_arg_pb.do_list()) - { - device::AbstractAgv::DOItem di; - di.id = d.id(); - di.status = d.status(); - req.script_args.do_list.push_back(di); - } - for (auto& di : s_arg_pb.di_list()) - { - device::AbstractAgv::DIItem d; - d.id = di.id(); - d.status = di.status(); - req.script_args.di_list.push_back(d); - } - } - - // -------- 原地旋转 -------- - if (pb_data.has_move_angle()) req.move_angle = pb_data.move_angle(); - if (pb_data.has_speed_w()) req.speed_w = pb_data.speed_w(); - if (pb_data.has_loc_mode()) req.loc_mode = pb_data.loc_mode(); - - // -------- 自由导航 -------- - if (pb_data.has_freego()) - { - auto& fg_pb = pb_data.freego(); - req.freeGo.x = fg_pb.x(); - req.freeGo.y = fg_pb.y(); - req.freeGo.theta = fg_pb.theta(); - } - - // 调用设备层 - device::AbstractAgv::RobotGoTargetRes res; - agv_dev->robotGoTarget(res, req); - - auto* out_data = response->mutable_data(); - out_data->set_ret_code(res.ret_code); - out_data->set_create_on(res.create_on); - out_data->set_err_msg(res.err_msg); - - auto* header = response->mutable_header(); - header->set_success(true); - header->set_error_message(""); - setCurrentTimestamp(header->mutable_timestamp()); - - LOG(INFO) << "[RobotGoTarget] finish ret_code=" << res.ret_code; - return grpc::Status::OK; - } - catch (std::exception& e) - { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(std::string("exception:") + e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - } - - // ===================== RobotControlStop ===================== - /** - * @brief 停止开环运动。 - * @param context 未使用。 - * @param request 包含设备 ID（无业务数据）。 - * @param response 返回停止结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotControlStop( - grpc::ServerContext* context, - const api::RobotControlStopCommand::Request* request, - api::RobotControlStopCommand::Feedback* response) { - - const auto& header = request->header(); - std::string device_id = header.device_id(); - auto* fb_header = response->mutable_header(); - - if (device_id.empty()) { - fb_header->set_error_message("Missing device_id"); - return grpc::Status::OK; - } - - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - fb_header->set_error_message("Device not found or not an AGV"); - return grpc::Status::OK; - } - - cmvr::device::AbstractAgv::RobotControlStopRes res; - agv->robotControlStop(res); - - auto* status = response->mutable_status(); - status->set_ret_code(res.ret_code); - status->set_create_on(res.create_on); - status->set_err_msg(res.err_msg); - - if (res.ret_code == 0) { - fb_header->set_error_message(""); - } else { - fb_header->set_error_message(res.err_msg.empty() ? "Stop motion failed" : res.err_msg); - } - - return grpc::Status::OK; - } - - // ===================== RobotTaskPause ===================== - /** - * @brief 暂停当前导航任务。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回暂停结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotTaskPause( - grpc::ServerContext* context, - const api::RobotTaskPauseCommand::Request* request, - api::RobotTaskPauseCommand::Feedback* response) { - - const auto& header = request->header(); - std::string device_id = header.device_id(); - auto* fb_header = response->mutable_header(); - - if (device_id.empty()) { - fb_header->set_error_message("Missing device_id"); - return grpc::Status::OK; - } - - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - fb_header->set_error_message("Device not found or not an AGV"); - return grpc::Status::OK; - } - - cmvr::device::AbstractAgv::RobotTaskPauseRes res; - try { - agv->robotTaskPause(res); - } catch (const std::exception& e) { - fb_header->set_error_message(std::string("Exception: ") + e.what()); - return grpc::Status::OK; - } - - auto* status = response->mutable_status(); - status->set_ret_code(res.ret_code); - status->set_create_on(res.create_on); - status->set_err_msg(res.err_msg); - - if (res.ret_code != 0) { - fb_header->set_error_message(res.err_msg.empty() ? "Pause task failed" : res.err_msg); - } else { - fb_header->set_error_message(""); - } - - return grpc::Status::OK; - } - - // ===================== RobotTaskResume ===================== - /** - * @brief 继续当前导航任务。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回继续结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotTaskResume( - grpc::ServerContext* context, - const api::RobotTaskResumeCommand::Request* request, - api::RobotTaskResumeCommand::Feedback* response) { - - const auto& header = request->header(); - std::string device_id = header.device_id(); - auto* fb_header = response->mutable_header(); - - if (device_id.empty()) { - fb_header->set_error_message("Missing device_id"); - return grpc::Status::OK; - } - - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - fb_header->set_error_message("Device not found or not an AGV"); - return grpc::Status::OK; - } - - cmvr::device::AbstractAgv::RobotTaskResumeRes res; - try { - agv->robotTaskResume(res); - } catch (const std::exception& e) { - fb_header->set_error_message(std::string("Exception: ") + e.what()); - return grpc::Status::OK; - } - - auto* status = response->mutable_status(); - status->set_ret_code(res.ret_code); - status->set_create_on(res.create_on); - status->set_err_msg(res.err_msg); - - if (res.ret_code != 0) { - fb_header->set_error_message(res.err_msg.empty() ? "Resume task failed" : res.err_msg); - } else { - fb_header->set_error_message(""); - } - - return grpc::Status::OK; - } - - // ===================== RobotTaskCancel ===================== - /** - * @brief 取消当前导航任务。 - * @param context 未使用。 - * @param request 包含设备 ID。 - * @param response 返回取消结果。 - */ - grpc::Status gRPCAGVServiceImpl::RobotTaskCancel( - grpc::ServerContext* context, - const api::RobotTaskCancelCommand::Request* request, - api::RobotTaskCancelCommand::Feedback* response) { - - const auto& header = request->header(); - std::string device_id = header.device_id(); - auto* fb_header = response->mutable_header(); - - if (device_id.empty()) { - fb_header->set_error_message("Missing device_id"); - return grpc::Status::OK; - } - - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - fb_header->set_error_message("Device not found or not an AGV"); - return grpc::Status::OK; - } - - cmvr::device::AbstractAgv::RobotTaskCancelRes res; - try { - agv->robotTaskCancel(res); - } catch (const std::exception& e) { - fb_header->set_error_message(std::string("Exception: ") + e.what()); - return grpc::Status::OK; - } - - auto* status = response->mutable_status(); - status->set_ret_code(res.ret_code); - status->set_create_on(res.create_on); - status->set_err_msg(res.err_msg); - - if (res.ret_code != 0) { - fb_header->set_error_message(res.err_msg.empty() ? "Cancel task failed" : res.err_msg); - } else { - fb_header->set_error_message(""); - } - - return grpc::Status::OK; - } - -} // namespace cmvr::service \ No newline at end of file diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index 0c4e8469..487fe705 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -17,7 +17,7 @@ using google::protobuf::util::TimeUtil; gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()) { - pinocchio_ik_ = std::make_shared("/home/cmvr/Projects/cmvr-es/model/xiaoyan_description/dual_arm.urdf" + pinocchio_ik_ = std::make_shared("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" ,"PELVIS_S" ,"R_WRIST_R_S"); pinocchio_ik_->init(); diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp index c2817179..dc689aff 100644 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -22,8 +22,6 @@ #include "service/grpc/include/grpc_hlc_service.h" #include "json/json.h" #include -#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(); @@ -121,7 +126,6 @@ void ServerRunner::threadMain() { auto biohand_service = std::make_unique(); auto humanoid_robot_service = std::make_unique(); auto hlc_service = std::make_unique(); - auto agv_service = std::make_unique(); 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) { diff --git a/dependency/x86/third_party/soem/include/soem/ec_base.h b/dependency/x86/third_party/soem/include/soem/ec_base.h deleted file mode 100644 index 3eafa121..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_base.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_coe.h b/dependency/x86/third_party/soem/include/soem/ec_coe.h deleted file mode 100644 index 45c889e5..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_coe.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_config.h b/dependency/x86/third_party/soem/include/soem/ec_config.h deleted file mode 100644 index c0291177..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_config.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_dc.h b/dependency/x86/third_party/soem/include/soem/ec_dc.h deleted file mode 100644 index d0472dd3..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_dc.h +++ /dev/null @@ -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 */ diff --git a/dependency/x86/third_party/soem/include/soem/ec_eoe.h b/dependency/x86/third_party/soem/include/soem/ec_eoe.h deleted file mode 100644 index 27ca216e..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_eoe.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_foe.h b/dependency/x86/third_party/soem/include/soem/ec_foe.h deleted file mode 100644 index 6348b08a..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_foe.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_main.h b/dependency/x86/third_party/soem/include/soem/ec_main.h deleted file mode 100644 index 70189802..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_main.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_options.h b/dependency/x86/third_party/soem/include/soem/ec_options.h deleted file mode 100644 index a6b3d7f9..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_options.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_print.h b/dependency/x86/third_party/soem/include/soem/ec_print.h deleted file mode 100644 index 920f3dac..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_print.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_soe.h b/dependency/x86/third_party/soem/include/soem/ec_soe.h deleted file mode 100644 index e6dbf3ee..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_soe.h +++ /dev/null @@ -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 diff --git a/dependency/x86/third_party/soem/include/soem/ec_type.h b/dependency/x86/third_party/soem/include/soem/ec_type.h deleted file mode 100644 index faca1707..00000000 --- a/dependency/x86/third_party/soem/include/soem/ec_type.h +++ /dev/null @@ -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 */ diff --git a/dependency/x86/third_party/soem/include/soem/nicdrv.h b/dependency/x86/third_party/soem/include/soem/nicdrv.h deleted file mode 100644 index 010d8efc..00000000 --- a/dependency/x86/third_party/soem/include/soem/nicdrv.h +++ /dev/null @@ -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 - -/** 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 diff --git a/dependency/x86/third_party/soem/include/soem/osal.h b/dependency/x86/third_party/soem/include/soem/osal.h deleted file mode 100644 index f88bce85..00000000 --- a/dependency/x86/third_party/soem/include/soem/osal.h +++ /dev/null @@ -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 -#include - -/* 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 diff --git a/dependency/x86/third_party/soem/include/soem/osal_defs.h b/dependency/x86/third_party/soem/include/soem/osal_defs.h deleted file mode 100644 index 3d6ad511..00000000 --- a/dependency/x86/third_party/soem/include/soem/osal_defs.h +++ /dev/null @@ -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 -#include - -// define if debug printf is needed -#ifdef EC_DEBUG -#include -#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 diff --git a/dependency/x86/third_party/soem/include/soem/soem.h b/dependency/x86/third_party/soem/include/soem/soem.h deleted file mode 100644 index e4d43a8e..00000000 --- a/dependency/x86/third_party/soem/include/soem/soem.h +++ /dev/null @@ -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 */ diff --git a/dependency/x86/third_party/soem/lib/libsoem.a b/dependency/x86/third_party/soem/lib/libsoem.a deleted file mode 100644 index 51103678..00000000 Binary files a/dependency/x86/third_party/soem/lib/libsoem.a and /dev/null differ diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto deleted file mode 100644 index 8cfe89a1..00000000 --- a/protos/cmvr/api/agv_command.proto +++ /dev/null @@ -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 str_kv = 1; ///< 字符串键值对 - map 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; - } -} \ No newline at end of file diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto deleted file mode 100644 index 521ac025..00000000 --- a/protos/cmvr/api/agv_service.proto +++ /dev/null @@ -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); -} \ No newline at end of file diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto deleted file mode 100644 index cb972f1d..00000000 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ /dev/null @@ -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; -} diff --git a/protos/cmvr/config/biohead_config/biohead_config.proto b/protos/cmvr/config/biohead_config/biohead_config.proto deleted file mode 100644 index 80c7d052..00000000 --- a/protos/cmvr/config/biohead_config/biohead_config.proto +++ /dev/null @@ -1,44 +0,0 @@ -// biohead.proto -syntax = "proto3"; - -package cmvr.config; - -// 眉毛配置 -message EyeBrowConfig { - string serial = 1; // 如 "64:0~3" - string offest = 2; // 偏移量，如 "90 90 90 90" - string jLmtUp = 3; // 上限，如 "90 170 155 110" - string jLmtLow = 4; // 下限，如 "20 77 90 20" -} - -// 眼睛配置 -message EyeConfig { - string serial = 1; // 如 "64:4~9" - string offest = 2; // 偏移量，如 "90 90 90 90 90 90" - string jLmtUp = 3; // 上限，如 "90 150 165 90 120 115" - string jLmtLow = 4; // 下限，如 "20 90 90 25 70 75" -} - -// 嘴巴配置 -message MouthConfig { - string serial = 1; // 如 "65:0~9" - string offest = 2; // 偏移量，如 "90 90 90 90 90 90 90 90 90 90" - string jLmtUp = 3; // 上限，如 "150 110 130 140 100 105 110 125 90 95" - string jLmtLow = 4; // 下限，如 "70 30 80 80 65 55 45 80 85 90" -} - -// BioHead配置 -message BioHeadConfig { - string id = 1; // 设备ID，如 "bio_head" - string serial = 2; // 串口路径，如 "/dev/ttyUSB0" - int32 ctrlFreq = 3; // 控制频率，如 50 - - EyeBrowConfig eyebrow = 4; // 眉毛配置 - EyeConfig eye = 5; // 眼睛配置 - MouthConfig mouth = 6; // 嘴巴配置 - bool enable = 7; // 是否启用 -} - -message HeadConfig { - repeated BioHeadConfig bioheads = 3; -} \ No newline at end of file diff --git a/protos/cmvr/config/monitor_config/monitor_config.proto b/protos/cmvr/config/monitor_config/monitor_config.proto deleted file mode 100644 index 368c4660..00000000 --- a/protos/cmvr/config/monitor_config/monitor_config.proto +++ /dev/null @@ -1,22 +0,0 @@ -syntax = "proto3"; - -package cmvr.config; - -// 文件夹监控配置 -message FolderConfig { - string fileDir = 1; // 文件夹路径 - int32 maxVolume = 2; // 最大容量（MB） -} - -// 磁盘监控配置 -message DiskMonitorConfig { - string id = 1; // 监控器ID，如 "file_monitor" - int32 freq = 2; // 监控频率（Hz） - repeated FolderConfig folders = 3; // 要监控的文件夹列表 - bool enable = 4; // 是否启用 -} - -// 监控管理器配置 -message MonitorManagerConfig { - DiskMonitorConfig disk_monitor = 1; // 磁盘监控配置 -} \ No newline at end of file diff --git a/protos/cmvr/config/robot_config/robot_config.proto b/protos/cmvr/config/robot_config/robot_config.proto deleted file mode 100644 index 1838f1eb..00000000 --- a/protos/cmvr/config/robot_config/robot_config.proto +++ /dev/null @@ -1,88 +0,0 @@ -syntax = "proto3"; - -package cmvr.config; - -// 电机配置 -message MotorConfig { - int32 id = 1; // 电机ID - string jointName = 2; // 关节名称 - float limitQLb = 3; // 位置下限 - float limitQUb = 4; // 位置上限 - float limitQd = 5; // 速度限制 -} - -// 通用的CAN设备配置（手臂、头部、腰部都可以复用） -message CanDeviceConfig { - string id = 1; // CAN ID - string devId = 2; // 设备ID - int32 channelId = 3; // 通道ID - bool enable = 4; // 是否启用 - string toolFrame = 5; // 工具坐标系（手臂需要，头/腰可选） - repeated MotorConfig motors = 6; // 电机列表 -} - -// CAN管理器配置 -message CanManagerConfig { - string id = 1; // CAN管理器ID - string devId = 2; // 设备ID - CanDeviceConfig left_arm = 3; // 左臂CAN - CanDeviceConfig right_arm = 4; // 右臂CAN - CanDeviceConfig head = 5; // 头部CAN - CanDeviceConfig waist = 6; // 腰部CAN -} - -// 人形机器人配置 -message HumanRobotConfig { - string id = 1; // 机器人ID，如 "hc01" - int32 dof = 2; // 自由度，如 14 - string urdf = 3; // URDF文件路径 - string baseLink = 4; // 基座link名称，如 "PELVIS_S" - string jointNames = 5; // 关节名称列表，逗号分隔 - string linkNames = 6; // link名称列表，逗号分隔 - int32 bufferSize = 7; // 缓冲区大小，如 50 - bool verbose = 8; // 是否详细输出 - int32 updFreq = 9; // 更新频率，如 500 - CanManagerConfig can_manager = 10; // CAN管理器配置 - bool enable = 11; // 是否启用 -} - -// EtherCAT主站配置 -message EthercatMasterConfig { - string interface = 1; // 网络接口，如 "eth0" - int32 cycle_time_us = 2; // 周期时间（微秒），默认1000 - bool enable = 3; // 是否启用 -} - -// EtherCAT电机驱动配置 -message EthercatMotorConfig { - int32 slave = 1; // 从站ID - string jointName = 2; // 关节名称 - float limitQLb = 3; // 位置下限 - float limitQUb = 4; // 位置上限 - float limitQd = 5; // 速度限制 -} - -// EtherCAT机器人配置 -message EthercatRobotConfig { - string id = 1; // 机器人ID - int32 dof = 2; // 自由度 - string urdf = 3; // URDF文件路径 - string baseLink = 4; // 基座link名称 - string jointNames = 5; // 关节名称列表，逗号分隔 - string linkNames = 6; // link名称列表，逗号分隔 - int32 bufferSize = 7; // 缓冲区大小 - bool verbose = 8; // 是否详细输出 - int32 updFreq = 9; // 更新频率（Hz） - bool enable = 10; // 是否启用 - - EthercatMasterConfig master = 11; // EtherCAT主站配置 - repeated EthercatMotorConfig motors = 12; // 电机配置列表 -} - - - -// 主配置 -message RobotConfig { - repeated HumanRobotConfig humanrobots = 1; - repeated EthercatRobotConfig ethercat_robots = 2; // 新增的EtherCAT机器人 -} \ No newline at end of file diff --git a/protos/cmvr/config/server_config/server_config.proto b/protos/cmvr/config/server_config/server_config.proto deleted file mode 100644 index 911b12ca..00000000 --- a/protos/cmvr/config/server_config/server_config.proto +++ /dev/null @@ -1,10 +0,0 @@ -syntax = "proto3"; -package cmvr.config; - -message GrpcServerConfig{ - string port = 1; -} - -message ServerConfig{ - GrpcServerConfig grpc_server = 1; -} \ No newline at end of file diff --git a/request.txt b/request.txt index 9e9809c7..9a4d60e2 100644 --- a/request.txt +++ b/request.txt @@ -33,6 +33,5 @@ third_party/modbus/3.1.11 third_party/visp/3.7.0 third_party/mainif/0.0.5 third_party/matplotplusplus/1.2.0 -third_party/soem