From 82ab7cd887e7d91849e88170f9a7840f7bf08f5b Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Wed, 29 Oct 2025 16:26:15 +0800 Subject: [PATCH] update robot controller --- config/dev_config.xml | 1 - include/hardware/can/can_group.h | 15 +- .../motor_protocol/abstractmotorprotocol.h | 36 +- .../can/motor_protocol/motorprotocolmanager.h | 27 +- .../can/motor_protocol/ti5motorprotocol.h | 100 +++- .../robot/controller/abstractcontroller.h | 14 +- .../robot/controller/cartesiancontroller.cpp | 15 + .../robot/controller/cartesiancontroller.h | 8 +- .../robot/controller/controller_manager.cpp | 53 ++ .../robot/controller/controller_manager.h | 15 +- .../controller/jointpositioncontroller.cpp | 24 +- .../controller/jointpositioncontroller.h | 9 +- src/hardware/can/can_group.cpp | 33 +- src/hardware/can/can_manager.cpp | 5 +- .../motor_protocol/motorprotocolmanager.cpp | 39 +- .../can/motor_protocol/ti5motorprotocol.cpp | 487 +++++++++++++++++- src/main.cpp | 8 +- 17 files changed, 826 insertions(+), 63 deletions(-) diff --git a/config/dev_config.xml b/config/dev_config.xml index 0f1ccf6f..4feb5aa1 100644 --- a/config/dev_config.xml +++ b/config/dev_config.xml @@ -6,7 +6,6 @@ - diff --git a/include/hardware/can/can_group.h b/include/hardware/can/can_group.h index a6671caf..f356795c 100644 --- a/include/hardware/can/can_group.h +++ b/include/hardware/can/can_group.h @@ -2,10 +2,10 @@ // Created by linbo on 2025/10/22. // -#ifndef CMVR_ES_CAN_GROUP_H -#define CMVR_ES_CAN_GROUP_H +#pragma once #include "motor_protocol/motorprotocolmanager.h" + namespace cmvr::hardware { class CanGroup @@ -14,15 +14,18 @@ namespace cmvr::hardware explicit CanGroup(const XmlNode &cfg); ~CanGroup() = default; + void init(); private: + XmlNode cfg_; std::string id_; unsigned int channelId_; bool enable_; std::shared_ptr motor_protocol_manager; + std::shared_ptr can_client_{nullptr}; + std::shared_ptr> can_receiver_{nullptr}; + std::shared_ptr> can_sender_{nullptr}; + std::shared_ptr> message_manager_{nullptr}; }; -} - - -#endif //CMVR_ES_CAN_GROUP_H \ No newline at end of file +} \ No newline at end of file diff --git a/include/hardware/can/motor_protocol/abstractmotorprotocol.h b/include/hardware/can/motor_protocol/abstractmotorprotocol.h index 15e89f78..ce9d090e 100644 --- a/include/hardware/can/motor_protocol/abstractmotorprotocol.h +++ b/include/hardware/can/motor_protocol/abstractmotorprotocol.h @@ -5,11 +5,18 @@ #ifndef CMVR_ES_ABSTRACTMOTORPROTOCOL_H #define CMVR_ES_ABSTRACTMOTORPROTOCOL_H #include "rapidxml/xml_parser.h" - +#include "cmvr/msgs/canopen.pb.h" +#include "cmvr/msgs/motor.pb.h" namespace cmvr::hardware { class AbstractMotorProtocol { + + public: + enum class CommProto : uint8_t { + CANOPEN = 1, + CUSTOM = 2 + }; public: explicit AbstractMotorProtocol(const XmlNode &cfg) { @@ -20,9 +27,32 @@ namespace cmvr::hardware limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f); limitQd = cfg.getAttrDefault("limitQd",3.0f); } - ~AbstractMotorProtocol() = default; + virtual ~AbstractMotorProtocol() = default; - private: + /** + * @brief 用于初始化与通讯协议相关的设置 + * @param node_id 电机Id + * @return + */ + virtual bool initNode(uint8_t node_id) = 0; + + virtual void setQ(uint8_t node_id, double angle_rad) = 0; + virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; + virtual msgs::RunMode getMode(uint8_t node_id) = 0; + virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; + virtual void setLimitQd(uint8_t node_id,double qd) = 0; + virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0; + virtual bool calibrateZeroQ(uint8_t node_id) = 0; + virtual bool reachedTargetQ(uint8_t node_id) = 0; + virtual void setQd(uint8_t node_id, double qd) = 0; + virtual void setQdd(uint8_t node_id,double qdd) = 0; + virtual void brake(uint8_t node_id) = 0; + virtual void torqueOff(uint8_t node_id) = 0; + + virtual double getQ(uint8_t node_id) = 0; + virtual double getQd(uint8_t node_id) = 0; + CommProto comm_proto{CommProto::CANOPEN}; + protected: XmlNode cfg_; std::string id_; std::string joint_name_; diff --git a/include/hardware/can/motor_protocol/motorprotocolmanager.h b/include/hardware/can/motor_protocol/motorprotocolmanager.h index e72efe10..2880b08d 100644 --- a/include/hardware/can/motor_protocol/motorprotocolmanager.h +++ b/include/hardware/can/motor_protocol/motorprotocolmanager.h @@ -8,18 +8,41 @@ #include #include #include "abstractmotorprotocol.h" +#include "devices/abstract_canbus.h" +#include "canbus/can_comm/can_sender.h" +#include "canbus/can_comm/can_receiver.h" +#include "canbus/can_comm/message_manager.h" +#include "cmvr/msgs/robot_detail.pb.h" namespace cmvr::hardware { class MotorProtocolManager { public: - explicit MotorProtocolManager(const XmlNode &cfg); + struct MotorInfo + { + std::string node_id; //电机ID + std::string jointName; //关节名称 + double limitQLb; //逆时针限位 + double limitQUb; //顺时针限位 + double limitQd; //加速度限制 + }; + public: + explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr> sender, + std::shared_ptr> manager); ~MotorProtocolManager() = default; - std::shared_ptr getMotorProtocol(const std::string &joint_name); + //根据协议类型获取协议实例对象 + std::shared_ptr getMotorProtocol(const std::string &protocolType); private: + std::shared_ptr > can_sender_{nullptr}; + std::shared_ptr > message_manager_{nullptr}; + + std::shared_ptr ti5Protocol_{nullptr}; + //协议类型，协议实例对象 std::unordered_map> motor_protocols_; + //协议类型，电机参数 + std::unordered_map> motors_; }; } diff --git a/include/hardware/can/motor_protocol/ti5motorprotocol.h b/include/hardware/can/motor_protocol/ti5motorprotocol.h index c797a04f..a9ed9e32 100644 --- a/include/hardware/can/motor_protocol/ti5motorprotocol.h +++ b/include/hardware/can/motor_protocol/ti5motorprotocol.h @@ -2,19 +2,109 @@ // Created by linbo on 2025/10/22. // -#ifndef CMVR_ES_TI5MOTOR_H -#define CMVR_ES_TI5MOTOR_H +#pragma once #include "rapidxml/xml_parser.h" #include "abstractmotorprotocol.h" +#include "cmvr/msgs/motor.pb.h" +#include "motor/motor_protocol_interface.h" +#include "devices/abstract_canbus.h" +#include "canbus/can_comm/can_receiver.h" +#include "canbus/can_comm/can_sender.h" +#include "canbus/can_comm/message_manager.h" +#include "cmvr/msgs/error_code.pb.h" +#include "cmvr/msgs/robot_detail.pb.h" + +#include "canbus/canopen/sdo_request_protocol.h" +#include "canbus/canopen/sync_protocol.h" +#include "canbus/canopen/nmt_request_protocol.h" +#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h" +#include namespace cmvr::hardware{ class Ti5MotorProtocol final : public AbstractMotorProtocol { public: explicit Ti5MotorProtocol(const XmlNode &cfg); + Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr> sender, + std::shared_ptr> manager); + ~Ti5MotorProtocol() override; + + bool initNode(uint8_t node_id) override; + + void setMode(uint8_t node_id, msgs::RunMode mode) 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; + + void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10); + void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, + msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10); + + void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); + void configPdo(uint8_t node_id); + + std::unique_ptr GetRobotDetail() { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + return data_ptr; + } + + msgs::RunMode getMode(uint8_t node_id) override { + return cur_mode_[node_id]; + } + private: + void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); + void setPPTargetPosByPdo(uint8_t node_id, int32_t pos); + void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos); + void configTPDO1(uint8_t node_id); + void configTPDO2(uint8_t node_id); + // 目标位置 607A + 控制字 6040 + void configRPDO1(uint8_t node_id, bool start); + bool 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; + } + private: + static constexpr double GearRatio = 101.0; // 电机减速比 + static constexpr double RADTODEG = 180.0 / M_PI; + std::shared_ptr can_client_{nullptr}; + // key node_id + std::unordered_map cur_mode_{}; + std::unordered_map last_Qd_{}; + std::unordered_map last_Qdd_{}; + std::shared_ptr > can_sender_{nullptr}; + std::shared_ptr > message_manager_{nullptr}; + + // nmt + device::NmtRequestProtocol *nmt_command_{nullptr}; + + //sync + device::SyncProtocol *sync_command_{nullptr}; + + // sdo + std::map *> sdo_commands_{}; + + // rpdo1 + std::map rpdo1_commands_{}; }; -} - -#endif //CMVR_ES_TI5MOTOR_H \ No newline at end of file +} \ No newline at end of file diff --git a/src/devices/robot/controller/abstractcontroller.h b/src/devices/robot/controller/abstractcontroller.h index 6e312c2d..a9815d01 100644 --- a/src/devices/robot/controller/abstractcontroller.h +++ b/src/devices/robot/controller/abstractcontroller.h @@ -2,8 +2,7 @@ // Created by linbo on 2025/10/23. // -#ifndef CMVR_ES_ABSTRACTCONTROLLER_H -#define CMVR_ES_ABSTRACTCONTROLLER_H +#pragma once #include @@ -25,13 +24,20 @@ namespace cmvr::device [[nodiscard]] ControllerState getState() const {return state_;} + //此处的Json中应该包含目标电机信息，电机id，canGroupId等其他必要参数，用以确定是调用哪个can实例发送消息 + // 还要包含操作内容，比如要执行的是直接控制每个电机位置，还是指定末端关节位置 + /* + {“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]} + **/ virtual void call(const Json::Value& json) = 0; + virtual void interrupt() = 0; + virtual void stop() = 0; protected: ControllerState state_; + double defaultSpeed_; + double defaultAcc_; }; } - -#endif //CMVR_ES_ABSTRACTCONTROLLER_H \ No newline at end of file diff --git a/src/devices/robot/controller/cartesiancontroller.cpp b/src/devices/robot/controller/cartesiancontroller.cpp index a6bd9b6e..64fdc41c 100644 --- a/src/devices/robot/controller/cartesiancontroller.cpp +++ b/src/devices/robot/controller/cartesiancontroller.cpp @@ -16,8 +16,23 @@ void CartesianController::call(const Json::Value& json) { if (state_ != ControllerState_Idle) return; + + + state_ = ControllerState_Executing; } + +void CartesianController::interrupt() +{ + state_ = ControllerState_Switching; + + + state_ = ControllerState_Idle; +} + void CartesianController::stop() { + + state_ = ControllerState_Idle; + } \ No newline at end of file diff --git a/src/devices/robot/controller/cartesiancontroller.h b/src/devices/robot/controller/cartesiancontroller.h index 3cd615f7..1546655d 100644 --- a/src/devices/robot/controller/cartesiancontroller.h +++ b/src/devices/robot/controller/cartesiancontroller.h @@ -2,8 +2,7 @@ // Created by linbo on 2025/10/24. // -#ifndef CMVR_ES_CARTESIANCONTROLLER_H -#define CMVR_ES_CARTESIANCONTROLLER_H +#pragma once #include "abstractcontroller.h" namespace cmvr::device @@ -13,12 +12,9 @@ namespace cmvr::device public: explicit CartesianController(const XmlNode& cfg); void call(const Json::Value& json) override; + void interrupt() override; void stop() override; private: }; } - - - -#endif //CMVR_ES_CARTESIANCONTROLLER_H \ No newline at end of file diff --git a/src/devices/robot/controller/controller_manager.cpp b/src/devices/robot/controller/controller_manager.cpp index 0a21b1a2..6470287d 100644 --- a/src/devices/robot/controller/controller_manager.cpp +++ b/src/devices/robot/controller/controller_manager.cpp @@ -54,3 +54,56 @@ void ControllerManager::create(const XmlNode& cfg) } } +void ControllerManager::switchMode(ControlManagerState state) +{ + //无论当前切换成哪种模式都应该停止控制器操作？ + if (activeController_) + { + activeController_->stop(); + } + //切换当前状态 + switch (state) + { + case ControlManagerState_Idle: + { + break; + } + case ControlManagerState_Command: + { + break; + } + case ControlManagerState_MinorFault: + { + break; + } + case ControlManagerState_MajorFault: + { + break; + } + case ControlManagerState_Teach: + { + break; + } + case ControlManagerState_EStop: + { + break; + } + default: + break; + } + state_ = state; + + +} + +ComponentGroup& ControllerManager::getComponentGroup(const std::string& id) +{ + if (componentGroups_.count(id)) + return componentGroups_[id]; + throw std::runtime_error("ComponentGroup not found"); +} + +std::shared_ptr ControllerManager::getActiveController() +{ + return activeController_; +} diff --git a/src/devices/robot/controller/controller_manager.h b/src/devices/robot/controller/controller_manager.h index c6262a22..364bcc8c 100644 --- a/src/devices/robot/controller/controller_manager.h +++ b/src/devices/robot/controller/controller_manager.h @@ -2,8 +2,7 @@ // Created by linbo on 2025/10/23. // -#ifndef CMVR_ES_CONTROLLER_MANAGER_H -#define CMVR_ES_CONTROLLER_MANAGER_H +#pragma once #include "abstractcontroller.h" namespace cmvr::device @@ -34,20 +33,22 @@ namespace cmvr::device [[nodiscard]] ControlManagerState getSate() const {return state_;} + [[nodiscard]] ComponentGroup& getComponentGroup(const std::string& id); // 清除错误信息 void clearError(); // 根据xml内容创建控制器组 void create(const XmlNode& cfg); + + void switchMode(ControlManagerState state); + + std::shared_ptr getActiveController(); private: ControlManagerState state_; std::unordered_map componentGroups_; + + std::shared_ptr activeController_{nullptr};//当前活动控制器 }; } - - - - -#endif //CMVR_ES_CONTROLLER_MANAGER_H \ No newline at end of file diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index cc68e132..cc61b178 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -3,20 +3,40 @@ // #include "jointpositioncontroller.h" +#include "hardware/can/can_manager.h" +#include "hardware_manager/hardware_manager.h" using namespace std; using namespace cmvr::device; +using namespace cmvr::hardware; + JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg) { } - void JointPositionController::call(const Json::Value& json) { if (state_ != ControllerState_Idle) return; + + + //解析jason，执行算法 + + + + //调用canmanager执行电机指令 + HardWareManager::getInstance(); } + +void JointPositionController::interrupt() +{ + state_ = ControllerState_Switching; + + + state_ = ControllerState_Idle; +} + void JointPositionController::stop() { - + state_ = ControllerState_Idle; } \ No newline at end of file diff --git a/src/devices/robot/controller/jointpositioncontroller.h b/src/devices/robot/controller/jointpositioncontroller.h index 5e37eb2b..183be116 100644 --- a/src/devices/robot/controller/jointpositioncontroller.h +++ b/src/devices/robot/controller/jointpositioncontroller.h @@ -2,8 +2,7 @@ // Created by linbo on 2025/10/24. // -#ifndef CMVR_ES_JOINTPOSITIONCONTROLLER_H -#define CMVR_ES_JOINTPOSITIONCONTROLLER_H +#pragma once #include "abstractcontroller.h" namespace cmvr::device @@ -13,12 +12,8 @@ namespace cmvr::device public: explicit JointPositionController(const XmlNode& cfg); void call(const Json::Value& json) override; + void interrupt() override; void stop() override; private: }; } - - - - -#endif //CMVR_ES_JOINTPOSITIONCONTROLLER_H \ No newline at end of file diff --git a/src/hardware/can/can_group.cpp b/src/hardware/can/can_group.cpp index 88ed417a..8510db40 100644 --- a/src/hardware/can/can_group.cpp +++ b/src/hardware/can/can_group.cpp @@ -7,18 +7,47 @@ using namespace std; using namespace cmvr::hardware; -CanGroup::CanGroup(const XmlNode &cfg) +CanGroup::CanGroup(const XmlNode &cfg):cfg_(cfg) { try { id_ = cfg.getAttrString("id"); channelId_ = cfg.getAttrDefault("channelId",0); enable_ = cfg.getAttrDefault("enable", false); - motor_protocol_manager = std::make_shared(cfg); + } catch (const exception &e) { LOG(FATAL) << "[CanGroup] (CanGroup): CanGroup initialization failed: " << e.what(); throw std::runtime_error("[CanGroup] (CanGroup): CanGroup create failed" + string(e.what())); } +} + + +void CanGroup::init() +{ + can_client_ = std::make_shared(cfg_); + can_sender_ = std::make_shared >(); + can_receiver_ = std::make_shared >(); + message_manager_ = std::make_shared >(); + + //初始化protocol协议实例 + can_client_->init(); + auto ret = can_sender_->Init(can_client_.get(), false); + if (ret != device::ErrorCode::OK) + LOG(ERROR) << "Failed to init " << id_ << " CAN sender."; + ret = can_receiver_->Init(can_client_.get(), message_manager_.get(), false); + if (ret != device::ErrorCode::OK) + LOG(ERROR) << "Failed to init " << id_ << " CAN receiver."; + can_client_->start(); + ret = can_sender_->Start(); + if (ret != device::ErrorCode::OK) + LOG(ERROR) << "Failed to start " << id_ << " CAN sender."; + + ret = can_receiver_->Start(); + if (ret != device::ErrorCode::OK) + LOG(ERROR) << "Failed to start " << id_ << " CAN receiver."; + + + motor_protocol_manager = std::make_shared(cfg_,can_sender_,message_manager_); } diff --git a/src/hardware/can/can_manager.cpp b/src/hardware/can/can_manager.cpp index d218d9c1..76e36357 100644 --- a/src/hardware/can/can_manager.cpp +++ b/src/hardware/can/can_manager.cpp @@ -18,7 +18,10 @@ CanManager::CanManager(const XmlNode& cfg) string id = node.getAttrString("id"); if (canGroups_.count(id)) throw runtime_error("[CanManager](CanManager): duplicate CanGroup id:" + id); - canGroups_[id] = std::make_shared(cfg); + auto canGroup = std::make_shared(cfg); + canGroup->init(); + canGroups_[id] = canGroup; + } } } diff --git a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp index 2c1b9f04..6ad98dd9 100644 --- a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp +++ b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp @@ -9,22 +9,31 @@ using namespace std; using namespace cmvr::hardware; - - -MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg) +MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr> sender, + std::shared_ptr> manager) { + can_sender_ = sender; + message_manager_ = manager; try { - for (auto &node: cfg.getChildren()){ - if (node.getNodeName() == "Ti5MotorProtocol") - { - std::string id = node.getAttrString("id"); - std::string joint_name = node.getAttrString("joint_name"); - if (motor_protocols_.count(joint_name)) - throw runtime_error("[MotorProtocolManager] (MotorProtocolManager): duplicate motorProtocol joint name:" + joint_name); - - motor_protocols_[joint_name] = std::make_shared(cfg); + if (cfg.hasChild("Ti5MotorProtocol")) + { + motor_protocols_["Ti5MotorProtocol"] = std::make_shared(cfg,can_sender_,message_manager_); + std::vector motors; + for (auto &node: cfg.getChildren()){ + if (node.getNodeName() == "Ti5MotorProtocol") + { + MotorInfo motor; + motor.node_id = node.getAttrString("id"); + motor.jointName = node.getAttrString("joint_name"); + motor.limitQd = node.getAttrDefault("limitQd",3.0f); + motor.limitQLb = node.getAttrDefault("limitQLb",3.14f); + motor.limitQUb = node.getAttrDefault("limitQUb",3.14f); + motors.emplace_back(motor); + } } + motors_["Ti5MotorProtocol"] = motors; } + } catch (const exception &e) { LOG(FATAL) << "[MotorProtocolManager] (MotorProtocolManager): MotorProtocolManager initialization failed: " << e.what(); @@ -33,9 +42,9 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg) } -std::shared_ptr MotorProtocolManager::getMotorProtocol(const std::string &joint_name) +std::shared_ptr MotorProtocolManager::getMotorProtocol(const std::string &protocolType) { - if (motor_protocols_.count(joint_name)) - return motor_protocols_[joint_name]; + if (motor_protocols_.count(protocolType)) + return motor_protocols_[protocolType]; return nullptr; } diff --git a/src/hardware/can/motor_protocol/ti5motorprotocol.cpp b/src/hardware/can/motor_protocol/ti5motorprotocol.cpp index 54e1f5a9..33308411 100644 --- a/src/hardware/can/motor_protocol/ti5motorprotocol.cpp +++ b/src/hardware/can/motor_protocol/ti5motorprotocol.cpp @@ -3,9 +3,494 @@ // #include "hardware/can/motor_protocol/ti5motorprotocol.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" +using namespace std; +using namespace cmvr::device; +using namespace cmvr::msgs; using namespace cmvr::hardware; Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg):AbstractMotorProtocol(cfg) { -} \ No newline at end of file +} + +Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr > sender, + std::shared_ptr > manager) + : AbstractMotorProtocol(cfg),can_sender_(sender), message_manager_(manager){ + + comm_proto = CommProto::CANOPEN; + + // 添加 message + + // NMT + message_manager_->AddSendProtocolData, false>(); + //sync + message_manager_->AddSendProtocolData, false>(); + + + // nmt + nmt_command_ = dynamic_cast *>( + message_manager_->GetMutableProtocolDataById(NmtRequestProtocol::ID)); + + + if (nmt_command_ == nullptr) { + LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; + } + can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true); + + // sync + sync_command_ = dynamic_cast *>( + message_manager_->GetMutableProtocolDataById(SyncProtocol::ID)); + if (sync_command_ == nullptr) { + LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; + } + can_sender_->AddMessage(sync_command_->ID, sync_command_, false); +} +Ti5MotorProtocol::~Ti5MotorProtocol() +{ + +} +bool Ti5MotorProtocol::initNode(uint8_t node_id) { + //nmt + message_manager_->AddRecvProtocolData, false>(node_id); + + //sdo + message_manager_->AddSendProtocolData, false>(node_id); + message_manager_->AddRecvProtocolData(node_id); + + //TPDO + message_manager_->AddRecvProtocolData(node_id); + message_manager_->AddRecvProtocolData(node_id); + + //RPDO + message_manager_->AddSendProtocolData(node_id); + + + + + sdo_commands_[node_id] = dynamic_cast *>( + message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::ID(node_id))); + + if (sdo_commands_[node_id] == nullptr) { + LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); + + + // pdo1 + rpdo1_commands_[node_id] = dynamic_cast( + message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id))); + + if (rpdo1_commands_[node_id] == nullptr) { + LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; + return ErrorCode::CANBUS_ERROR; + } + can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); + + return ErrorCode::OK; +} + + +void Ti5MotorProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, + uint32_t data, uint32_t delay_ms) { + sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data); + can_sender_->Update(sdo_commands_[node_id]->ID()); + std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); +} + +void Ti5MotorProtocol::setQ(uint8_t node_id, double angle_rad) { + auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; + + switch (cur_mode_[node_id]) { + case RUN_MODE_CYCLIC_SYNC_POSITION: + setCSPTargetPosByPdo(node_id, static_cast(cmd)); + break; + case RUN_MODE_PROFILE_POSITION: + setPPTargetPosByPdo(node_id, static_cast(cmd)); + break; + default: + break; + } +} + + +void Ti5MotorProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { + controlword_t cw = {}; + cw.switch_on = 1; + cw.enable_voltage = 1; + cw.enable_operation = 1; + cw.quick_stop = 1; + cw.change_set_immediately = 1; + + // 1. 设置目标位置 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos); + + + // 2. 设置触发位（bit4 = 1） + cw.new_set_point = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + + + // 3. 清除触发位（bit4 = 0），准备下一次触发 + cw.new_set_point = 0; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); +} + +void Ti5MotorProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) { + // 触发目标位置运动 + controlword_t cw; + cw.value = 0x0F; + cw.new_set_point = 1; + cw.change_set_immediately = 1; + + rpdo1_commands_[node_id]->SetTargetPos(pos); + rpdo1_commands_[node_id]->SetCtrlWord(cw.value); + can_sender_->Update(rpdo1_commands_[node_id]->ID()); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + cw.new_set_point = 0; + rpdo1_commands_[node_id]->SetCtrlWord(cw.value); + can_sender_->Update(rpdo1_commands_[node_id]->ID()); +} + +void Ti5MotorProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) { + rpdo1_commands_[node_id]->SetTargetPos(pos); + rpdo1_commands_[node_id]->SetCtrlWord(0x0F); + can_sender_->Update(rpdo1_commands_[node_id]->ID()); +} + + +void Ti5MotorProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { + + cur_mode_[node_id] = mode; + // 1 : 先设置模式 + auto data = static_cast(mode); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + + + // 2 : 状态机步进 —— Shutdown（0x06） + controlword_t cw = {}; + cw.quick_stop = 1; + cw.enable_voltage = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + + + // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） + cw.switch_on = 1; + cw.enable_operation = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + + + switch (mode) { + case RUN_MODE_PROFILE_POSITION: { + // 4 : 设置目标位置（为当前位置） + auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + + + // 5 : 触发位置运动（new_set_point 翻转） + cw.new_set_point = 1; + cw.change_set_immediately = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + + // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标） + cw.new_set_point = 0; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + break; + } + + case RUN_MODE_CYCLIC_SYNC_POSITION: { + // 设置目标位置为当前位置 + auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + + //3 : 使能 15 + cw.enable_operation = 1; + cw.switch_on = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + break; + } + + case RUN_MODE_PROFILE_VELOCITY: { + cw.enable_operation = 1; + cw.switch_on = 1; + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + break; + } + default: + + break; + } +} + +void Ti5MotorProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { + nmt_command_->RequestService(node_id, command); + can_sender_->Update(nmt_command_->ID); + std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); +} + + +void Ti5MotorProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); +} + + +void Ti5MotorProtocol::configTPDO1(uint8_t node_id) { + //TDPO1 配置 状态字 和 控制字 + // 1: 失能 pdo + uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0); + + // 2: 配置为异步 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + + // 3：配置约束时间 unit:0.1ms + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10); + + // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0); + + // 5 ：映射控制字 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1, + CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + + //6 : 映射状态字 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2, + STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); + + //7 : 映射模式 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3, + MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); + + //8 映射错误码 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4, + ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); + + //9 写入该PDO映射对象总个数 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4); + + //10 使能 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); +} + + +void Ti5MotorProtocol::configTPDO2(uint8_t node_id) { + // 1: 失能 pdo + uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0); + + // 2: 配置为异步 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + + // 3：配置约束时间 unit:0.1ms + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100); + + // 4 : 配置周期发送时间 unit : ms + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0); + + // 5 ：映射当前位置 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1, + ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); + + //6 : 映射当前速度 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2, + ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32); + + //9 写入该PDO映射对象总个数 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2); + + //10 使能 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); +} + +void Ti5MotorProtocol::configRPDO1(uint8_t node_id, bool start) { + // 1: 失能 pdo + uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0); + + // 2: 配置为 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + + // // 3：配置约束时间 unit:0.1ms + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10); + // + // // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0); + + // 5 ：映射位置 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1, + TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); + + //6 : 映射控制字 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, + CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + + + if (start) { + //7 写入该PDO映射对象总个数 + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + + //8 使能 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); + } +} + + +void Ti5MotorProtocol::configPdo(uint8_t node_id) { + configTPDO1(node_id); + configTPDO2(node_id); + configRPDO1(node_id, true); + +} + +void Ti5MotorProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) { + auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; + auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); +} + +void Ti5MotorProtocol::setLimitQd(uint8_t node_id,double qd) { + auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed); +} + +void Ti5MotorProtocol::setLimitQ(uint8_t node_id, double ub, double lb) { + ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0; + lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0; + + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); +} + + +bool Ti5MotorProtocol::calibrateZeroQ(uint8_t node_id) { + + // 0: 设置控制字为 0x06，确保停机状态 + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06,1000); + + // 1: 清除偏置值 0x2008 ← 0 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + + // 2: 等待确认清除成功 + seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + if (!waitUntil([&]() { + return GetRobotDetail()->motors().at(node_id).position_offset() == 0; + }, 1000)) { + LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; + return false; + } + + // 3: 读取当前位置 0x6064 + seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); + + // 4: 将当前位置写入偏置寄存器 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); + + // 5: 保存参数到永久区（0x2000 ← 1） + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1,100); + + + // 6: 确认写入成功 + seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); + if (!waitUntil([&]() { + return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; + }, 500)) { + return false; + LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; + } + + return true; +} + +void Ti5MotorProtocol::brake(uint8_t node_id) { + + // // 开机未使能电机时调用 + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // 6 抱闸 0 ： 立即停机 自由 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,100); + + // 必须要发送 0xf 才能按照6085中设定的减速度减速 + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); +} + +bool Ti5MotorProtocol::reachedTargetQ(uint8_t node_id) { + statusword_t st{}; + st.value = GetRobotDetail()->motors().at(node_id).status_word(); + return st.target_reached == 1 ; +} + +void Ti5MotorProtocol::setQd(uint8_t node_id, double qd) { + auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; + switch (cur_mode_[node_id]) { + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + case msgs::RUN_MODE_PROFILE_POSITION: { + auto it = last_Qd_.find(node_id); + if (it == last_Qd_.end() || it->second != speed) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),0); + last_Qd_[node_id] = speed; + } + break; + } + case msgs::RUN_MODE_PROFILE_VELOCITY: { + // 在速度模式下，直接设置目标速度 + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0); + break; + } + default: + break; + } +} + +void Ti5MotorProtocol::setQdd(uint8_t node_id, double qdd) { + uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0); + auto it = last_Qdd_.find(node_id); + if (it == last_Qdd_.end() || it->second != accel) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); + last_Qdd_[node_id] = accel; + } +} + +void Ti5MotorProtocol::torqueOff(uint8_t node_id) { + // 0 ： 立即停机 自由 + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,20); + // 必须要发送 0xf 才能按照6085中设定的减速度减速 + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + + // 停机之后，要重新使能？ + // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED; +} + + +double Ti5MotorProtocol::getQ(uint8_t node_id) { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + auto cnt = data_ptr->motors().at(node_id).position(); + return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); +} + +double Ti5MotorProtocol::getQd(uint8_t node_id) { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + auto cnt = data_ptr->motors().at(node_id).speed(); + return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); +} + + + diff --git a/src/main.cpp b/src/main.cpp index 831d6dda..2753330a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,6 +8,7 @@ #include #include #include "device_manager/device_manager.h" +#include "hardware_manager/hardware_factory.h" #include "monitor/monitor_manager.h" #include "service/grpc_service/grpc_camera_service.h" #include "service/grpc_service/grpc_system_service.h" @@ -22,16 +23,21 @@ #include "service/http_service/httpclient.h" #include "json/json.h" +#include "hardware_manager/hardware_manager.h" + void runServer(const XmlNode &cfg){ using namespace cmvr::device; using namespace cmvr::service; using namespace cmvr::monitor; - + using namespace cmvr::hardware; if (!cfg.hasChild("DeviceManager")){ LOG(ERROR) << "Device Manager node not found"; return; } try { + auto hardware_cfg = cfg.getChild("HardwareManager"); + HardWareManager::getInstance(hardware_cfg); + auto dmgr_cfg = cfg.getChild("DeviceManager"); DeviceManager::getInstance(dmgr_cfg);