feat(controller): 添加运动切换命令处理和多种运动控制功能

添加对运动切换命令(CheckMode/SelectMode/ReleaseMode)的处理支持
在Controller类中新增多种运动控制功能接口
更新README文档以包含新增的运动切换命令和功能说明

src/custom_robot.cpp

@@ -18,7 +18,6 @@ CustomRobot::CustomRobot()
     , mqttClient_(nullptr)
     , running_(false)
     , initialized_(false) {
-}
     config_.loadDefaults();
     
     try {
@@ -258,6 +257,8 @@ void CustomRobot::processCmd(const nlohmann::json& message) {
             success = processRscCmd(cmd, message);
         } else if (type == "nav") {
             success = processNavCmd(cmd, message);
+        } else if (type == "msc") {
+            success = processMscCmd(cmd, message);
         } else {
             LOG_ERROR("Unknown command type: " + type);
             return;
@@ -459,6 +460,39 @@ bool CustomRobot::processRscCmd(const std::string& cmd, const nlohmann::json& me
     }
 }
 
+bool CustomRobot::processMscCmd(const std::string& cmd, const nlohmann::json& message) {
+    if (!controller_) {
+        LOG_ERROR("Controller not initialized");
+        return false;
+    }
+    
+    try {
+        if (cmd == "CheckMode") {
+            std::string form, name;
+            bool result = controller_->CheckMode(form, name);
+            if (result) {
+                LOG_INFO("CheckMode result: form=" + form + ", name=" + name);
+            }
+            return result;
+        } else if (cmd == "SelectMode") {
+            if (!message.contains("param") || !message["param"].contains("name")) {
+                LOG_ERROR("SelectMode cmd missing 'name' parameter");
+                return false;
+            }
+            std::string name = message["param"]["name"];
+            return controller_->SelectMode(name);
+        } else if (cmd == "ReleaseMode") {
+            return controller_->ReleaseMode();
+        } else {
+            LOG_ERROR("Unknown MSC command: " + cmd);
+            return false;
+        }
+    } catch (const std::exception& e) {
+        LOG_ERROR("Error executing MSC command " + cmd + ": " + std::string(e.what()));
+        return false;
+    }
+}
+
 void CustomRobot::printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus) {
     std::string filterDesc = "All Services";
     if (filterStatus == 0) {