update robot controller

config/dev_config.xml

@@ -1,4 +1,4 @@
-<CMVR-ES>
+    <CMVR-ES>
     <Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
     <Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
 
@@ -6,6 +6,7 @@
         <CanManager>
             <CanGroup id="LeftArmCAN" channelId="0" enable="true">
                 <MotorProtocolManager>
+<!--                    这里每个协议节点中配置的参数应该是can通信协议中可修改的默认配置，比如钛虎电机默认的加速度，速度，限位-->
                     <Ti5MotorProtocol id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                     <Ti5MotorProtocol id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                     <Ti5MotorProtocol id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

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<ControllerManager> controller_manager_;
     };
 }
 

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)
+add_library(cmvr_es::device::controller_manager ALIAS controller_manager)

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_;
     };
 }
 

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()
+{
+
+}

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

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;
+
+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<JointPositionController>(controller_node);
+                }
+                else if (controller_node.getNodeName() == "CartesianController")
+                {
+                    group.controllers_["CartesianController"] = std::make_shared<CartesianController>(controller_node);
+                }
+            }
+
+            componentGroups_[group.id] = group;
+        }
+    }
+}
+

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<std::string> CanGroupIDs;
+        std::unordered_map<std::string, std::shared_ptr<AbstractController>> 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<std::string,ComponentGroup> componentGroups_;
     };
 }
 

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()
+{
+
+}

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