update can_group

include/hardware/can/can_group.h

@@ -18,6 +18,8 @@ namespace cmvr::hardware
 
         std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
         uint8_t getNodeId(const std::string& joint_name);
+
+        const MotorProtocolManager::MotorInfo& getMotorInfo(const std::string& joint_name);
     private:
         XmlNode cfg_;
         std::string id_;

include/hardware/can/motor_protocol/motorprotocolmanager.h

@@ -26,6 +26,7 @@ namespace cmvr::hardware
             double limitQLb;        //逆时针限位
             double limitQUb;        //顺时针限位
             double limitQd;         //加速度限制
+            std::shared_ptr<AbstractMotorProtocol> motorProtocol;
         };
     public:
         explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
@@ -35,6 +36,8 @@ namespace cmvr::hardware
         uint8_t getNodeId(const std::string& joint_name);
         //根据协议类型获取协议实例对象
         std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
+
+        const MotorInfo& getMotorInfo(const std::string& joint_name);
     private:
         std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
         std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};

src/devices/robot/controller/jointpositioncontroller.cpp

@@ -24,7 +24,6 @@ void JointPositionController::call(const Json::Value& json)
     /*
         "params":{
             “canGroupId”:"leftArm",
-            "protocolType":"Ti5MotorProtocol",
             "motors":[{"joint_name":""},{"joint_name":""}]
         },
         "operate":{}
@@ -34,11 +33,9 @@ void JointPositionController::call(const Json::Value& json)
     std::string protocolType;
     if (json.isMember("params"))
     {
-        Json::Value params = json["params"];
+        const Json::Value& params = json["params"];
         if (params.isMember("canGroupId"))
             canGroupId = params["canGroupId"].asString();
-        if (params.isMember("protocolType"))
-            protocolType = params["protocolType"].asString();
         if (params.isMember("motors"))
         {
             for (int i = 0; i < params["motors"].size(); i++)
@@ -52,10 +49,11 @@ void JointPositionController::call(const Json::Value& json)
     {
 
     }
+    //最后计算好了通过下面的接口调用电机通信
+    std::string joint_name ="";
+    auto motorInfo = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorInfo(joint_name);
+    motorInfo.motorProtocol->setQ(motorInfo.node_id,0.1);
 
-    //调用canmanager执行电机指令
-    auto protocol = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorProtocol(protocolType);
-    //这里通过joint_name获取node_id?
 }
 
 void JointPositionController::interrupt()

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -320,14 +320,27 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
         }
         else if (state == ControlManagerState_Command)
         {
-            //当前有命令正在执行，判断优先级？
             //先获取当前正在执行的控制器
-            auto controller = controller_manager_->getActiveController();
+            auto activeController = controller_manager_->getActiveController();
+            if (activeController)
+            {
+                activeController->stop();
+            }
+            auto group = controller_manager_->getComponentGroup("leftArm");
+            if (!group.controllers_.count("JointPositionCtrl"))
+                throw runtime_error("Controller not found");
+            auto controller = group.controllers_["JointPositionCtrl"];
+
+
             Json::Value callJson;
             Json::Value paramsJson;
             Json::Value operateJson;
             //拆分cmd，构建json内容
+            paramsJson["canGroupId"] = group.CanGroupIDs[0];
+            callJson["params"] = paramsJson;
+            callJson["operate"] = operateJson;
 
+            controller->call(callJson);
         }
 
         for (const auto &j: cmd) {

src/hardware/can/can_group.cpp

@@ -61,3 +61,7 @@ uint8_t CanGroup::getNodeId(const std::string& joint_name)
     return motor_protocol_manager->getNodeId(joint_name);
 }
 
+const MotorProtocolManager::MotorInfo& CanGroup::getMotorInfo(const std::string& joint_name)
+{
+    return motor_protocol_manager->getMotorInfo(joint_name);
+}

src/hardware/can/motor_protocol/motorprotocolmanager.cpp

@@ -32,6 +32,7 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<de
                     motor.limitQd = node.getAttrDefault("limitQd",3.0f);
                     motor.limitQLb = node.getAttrDefault("limitQLb",3.14f);
                     motor.limitQUb = node.getAttrDefault("limitQUb",3.14f);
+                    motor.motorProtocol = motor_protocols_["Ti5MotorProtocol"];
                     motors.emplace_back(motor);
                 }
             }
@@ -69,4 +70,21 @@ uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name)
         }
     }
     return node_id;
+}
+
+const MotorProtocolManager::MotorInfo& MotorProtocolManager::getMotorInfo(const std::string& joint_name)
+{
+    if (motors_.count("Ti5MotorProtocol"))
+    {
+        const auto motors = motors_["Ti5MotorProtocol"];
+        for (auto &motor: motors)
+        {
+            if (motor.jointName == joint_name)
+            {
+                return motor;
+            }
+        }
+    }
+
+    return MotorInfo();
 }