diff --git a/config/dev_config.xml b/config/dev_config.xml index ef6d3d81..0f1ccf6f 100644 --- a/config/dev_config.xml +++ b/config/dev_config.xml @@ -1,4 +1,4 @@ - + @@ -6,6 +6,7 @@ + diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index 335537df..c90006b6 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -11,6 +11,7 @@ #include "utils/solver/qp_solver.h" #include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/motor.pb.h" +#include "robot/controller/controller_manager.h" namespace cmvr::device{ @@ -122,6 +123,8 @@ namespace cmvr::device{ int dof_{}; RobotState state_{}; std::string toolFrame_; + + std::shared_ptr controller_manager_; }; } diff --git a/src/devices/robot/controller/CMakeLists.txt b/src/devices/robot/controller/CMakeLists.txt index e084d2d0..da8ecb81 100644 --- a/src/devices/robot/controller/CMakeLists.txt +++ b/src/devices/robot/controller/CMakeLists.txt @@ -1,6 +1,10 @@ -add_library(controller SHARED controller_manager.cpp) +add_library(controller_manager SHARED + jointpositioncontroller.cpp + cartesiancontroller.cpp + controller_manager.cpp +) -target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories(controller_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -add_library(cmvr_es::device::controller ALIAS controller) \ No newline at end of file +add_library(cmvr_es::device::controller_manager ALIAS controller_manager) \ No newline at end of file diff --git a/src/devices/robot/controller/abstractcontroller.h b/src/devices/robot/controller/abstractcontroller.h index d22f69dd..5c671fdf 100644 --- a/src/devices/robot/controller/abstractcontroller.h +++ b/src/devices/robot/controller/abstractcontroller.h @@ -11,10 +11,26 @@ namespace cmvr::device { + enum ControllerState + { + ControllerState_Idle, //默认 + ControllerState_Executing, //正在执行命令 + ControllerState_Switching //命令被中断，正在切换 + }; class AbstractController { public: - explicit AbstractController(); + explicit AbstractController(const XmlNode& cfg); + virtual ~AbstractController(); + + [[nodiscard]] ControllerState getState() const {return state_;} + + virtual void call(std::string json) = 0; + + virtual void stop() = 0; + + private: + ControllerState state_; }; } diff --git a/src/devices/robot/controller/cartesiancontroller.cpp b/src/devices/robot/controller/cartesiancontroller.cpp new file mode 100644 index 00000000..918cdd83 --- /dev/null +++ b/src/devices/robot/controller/cartesiancontroller.cpp @@ -0,0 +1,22 @@ +// +// Created by linbo on 2025/10/24. +// + +#include "cartesiancontroller.h" + +using namespace std; +using namespace cmvr::device; +CartesianController::CartesianController(const XmlNode& cfg):AbstractController(cfg) +{ + +} + + +void CartesianController::call(std::string json) +{ + +} +void CartesianController::stop() +{ + +} \ No newline at end of file diff --git a/src/devices/robot/controller/cartesiancontroller.h b/src/devices/robot/controller/cartesiancontroller.h new file mode 100644 index 00000000..26560c9e --- /dev/null +++ b/src/devices/robot/controller/cartesiancontroller.h @@ -0,0 +1,23 @@ +// +// Created by linbo on 2025/10/24. +// + +#ifndef CMVR_ES_CARTESIANCONTROLLER_H +#define CMVR_ES_CARTESIANCONTROLLER_H +#include "abstractcontroller.h" + +namespace cmvr::device +{ + class CartesianController final:public AbstractController + { + public: + explicit CartesianController(const XmlNode& cfg); + virtual void call(std::string json) override; + virtual 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 98846855..b2872471 100644 --- a/src/devices/robot/controller/controller_manager.cpp +++ b/src/devices/robot/controller/controller_manager.cpp @@ -4,5 +4,52 @@ #include "controller_manager.h" +#include "jointpositioncontroller.h" +#include "cartesiancontroller.h" using namespace std; -using namespace cmvr::device; \ No newline at end of file +using namespace cmvr::device; + +ControllerManager::ControllerManager(const XmlNode& cfg):state_(ControlManagerState_Idle) +{ + +} + +void ControllerManager::clearError() +{ + state_ = ControlManagerState_Idle; +} + +void ControllerManager::create(const XmlNode& cfg) +{ + auto children = cfg.getChildren(); + for (auto& child : children) + { + if (child.getNodeName() == "ComponentGroup") + { + ComponentGroup group; + group.id = child.getAttrString("id"); + // CanGroupID可能记录了多个cangroupID + std::string cangroupIDs = child.getAttrString("CanGroupID"); + std::istringstream iss(cangroupIDs); + std::string id; + while (std::getline(iss, id, ',')) { + group.CanGroupIDs.push_back(id); + } + auto controller_nodes = child.getChildren(); + for (auto& controller_node : controller_nodes) + { + if (controller_node.getNodeName() == "JointPositionCtrl") + { + group.controllers_["JointPositionCtrl"] = std::make_shared(controller_node); + } + else if (controller_node.getNodeName() == "CartesianController") + { + group.controllers_["CartesianController"] = std::make_shared(controller_node); + } + } + + componentGroups_[group.id] = group; + } + } +} + diff --git a/src/devices/robot/controller/controller_manager.h b/src/devices/robot/controller/controller_manager.h index eae4319c..facc6ae6 100644 --- a/src/devices/robot/controller/controller_manager.h +++ b/src/devices/robot/controller/controller_manager.h @@ -6,16 +6,44 @@ #define CMVR_ES_CONTROLLER_MANAGER_H #include "abstractcontroller.h" - namespace cmvr::device { + + enum ControlManagerState + { + ControlManagerState_Idle, //默认 + ControlManagerState_Command, //指令模式 + ControlManagerState_MinorFault, //次要错误，可以继续运行 + ControlManagerState_MajorFault, //主要错误，不能继续运行 + ControlManagerState_Teach, //示教模式 + ControlManagerState_EStop //急停模式 + }; + + struct ComponentGroup + { + std::string id; + std::vector CanGroupIDs; + std::unordered_map> controllers_; + }; + class ControllerManager { public: - explicit ControllerManager(); + explicit ControllerManager(const XmlNode& cfg); ~ControllerManager() = default; + [[nodiscard]] ControlManagerState getSate() const {return state_;} + + // 清除错误信息 + void clearError(); + + + void create(const XmlNode& cfg); private: + + ControlManagerState state_; + + std::unordered_map componentGroups_; }; } diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp new file mode 100644 index 00000000..8b5dd998 --- /dev/null +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -0,0 +1,21 @@ +// +// Created by linbo on 2025/10/24. +// + +#include "jointpositioncontroller.h" +using namespace std; +using namespace cmvr::device; +JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg) +{ + +} + + +void JointPositionController::call(std::string json) +{ + +} +void JointPositionController::stop() +{ + +} \ No newline at end of file diff --git a/src/devices/robot/controller/jointpositioncontroller.h b/src/devices/robot/controller/jointpositioncontroller.h new file mode 100644 index 00000000..979826ac --- /dev/null +++ b/src/devices/robot/controller/jointpositioncontroller.h @@ -0,0 +1,24 @@ +// +// Created by linbo on 2025/10/24. +// + +#ifndef CMVR_ES_JOINTPOSITIONCONTROLLER_H +#define CMVR_ES_JOINTPOSITIONCONTROLLER_H + +#include "abstractcontroller.h" +namespace cmvr::device +{ + class JointPositionController final:public AbstractController + { + public: + explicit JointPositionController(const XmlNode& cfg); + virtual void call(std::string json) override; + virtual void stop() override; + private: + }; +} + + + + +#endif //CMVR_ES_JOINTPOSITIONCONTROLLER_H \ No newline at end of file