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

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

README.md

@@ -109,7 +109,7 @@ The robot communicates via MQTT using the following topic structure:
 
 ### Command Topics
 
-- `unitree/go2/cmd`: All commands (sport, navigation, robot state, obstacle avoidance)
+- `unitree/go2/cmd`: All commands (sport, navigation, robot state, obstacle avoidance, motion switcher)
 
 Commands are sent to the command topic with a JSON payload specifying the command type and parameters.
 
@@ -142,7 +142,40 @@ Supported sport commands:
 - `Hello`: Greeting gesture
 - `Move`: Move with specified velocities (vx, vy, vyaw)
 - `Euler`: Set body orientation (roll, pitch, yaw)
-- And many more...
+- `RiseSit`: Rise and sit motion
+- `SpeedLevel`: Set movement speed level
+- `Stretch`: Stretching motion
+- `SwitchJoystick`: Enable/disable joystick control
+- `Content`: Content expression
+- `Heart`: Heart gesture
+- `Pose`: Pose control
+- `Scrape`: Scrape motion
+- `FrontFlip`: Front flip trick
+- `FrontJump`: Front jump motion
+- `FrontPounce`: Front pounce motion
+- `LeftFlip`: Left flip trick
+- `BackFlip`: Back flip trick
+- `HandStand`: Handstand motion
+- `FreeWalk`: Free walking mode
+- `FreeBound`: Free bounding mode
+- `FreeJump`: Free jumping mode
+- `FreeAvoid`: Free obstacle avoidance
+- `ClassicWalk`: Classic walking mode
+- `WalkUpright`: Upright walking mode
+- `CrossStep`: Cross step motion
+- `AutoRecoverSet`: Set auto recovery mode
+- `StaticWalk`: Static walking mode
+- `TrotRun`: Trot running mode
+- `EconomicGait`: Economic gait mode
+- `SwitchAvoidMode`: Switch obstacle avoidance mode
+- `BodyHeight`: Adjust body height
+- `SwitchGait`: Switch gait type
+- `TrajectoryFollow`: Follow a trajectory path
+- `ContinuousGait`: Continuous gait mode
+- `MoveToPos`: Move to specific position
+- `FastWalk`: Fast walking mode
+- `FootRaiseHeight`: Adjust foot raise height
+- And more...
 
 ### Navigation Commands
 
@@ -185,6 +218,102 @@ Supported obstacle avoidance commands:
 - `MoveToAbsolutePosition`: Move to absolute position
 - `MoveToIncrementPosition`: Move to increment position
 
+### Motion Switcher Commands
+
+Motion switcher commands allow switching between different motion modes of the robot:
+
+```json
+{
+  "type": "msc",
+  "cmd": "SelectMode",
+  "param": {
+    "mode": "ai"
+  }
+}
+```
+
+Supported motion switcher commands:
+- `CheckMode`: Check the current motion mode and robot form
+- `SelectMode`: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w)
+- `ReleaseMode`: Release the current motion mode
+
+#### CheckMode
+
+Check the current motion mode and robot form.
+
+Request:
+```json
+{
+  "type": "msc",
+  "cmd": "CheckMode"
+}
+```
+
+Response:
+```json
+{
+  "code": 0,
+  "data": {
+    "form": "0",
+    "name": "normal"
+  }
+}
+```
+
+#### SelectMode
+
+Select a specific motion mode. Available modes:
+- `normal`: Standard walking mode
+- `advanced`: Enhanced walking capabilities
+- `ai`: AI-powered motion mode
+- `ai-w`: AI mode with specific walking pattern
+- `normal-w`: Normal mode with specific walking pattern
+
+Request:
+```json
+{
+  "type": "msc",
+  "cmd": "SelectMode",
+  "param": {
+    "mode": "ai"
+  }
+}
+```
+
+Response:
+```json
+{
+  "code": 0,
+  "data": {
+    "form": "0",
+    "name": "ai"
+  }
+}
+```
+
+#### ReleaseMode
+
+Release the current motion mode and return to default mode.
+
+Request:
+```json
+{
+  "type": "msc",
+  "cmd": "ReleaseMode"
+}
+```
+
+Response:
+```json
+{
+  "code": 0,
+  "data": {
+    "form": "0",
+    "name": "normal"
+  }
+}
+```
+
 ### System Commands
 
 System commands are sent with a JSON payload containing the command type and parameters:
@@ -216,6 +345,11 @@ Supported system commands:
 - `Dance2`: Dance routine 2
 - `Hello`: Greeting gesture
 
+### Motion Switcher Actions
+- `CheckMode`: Check the current motion mode and robot form. Returns the current motion mode and robot form information.
+- `SelectMode`: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w). Switches the robot to the specified motion mode.
+- `ReleaseMode`: Release the current motion mode and return to default mode. Releases the currently selected motion mode.
+
 ### Navigation Actions
 - `start_mapping`: Start SLAM mapping
 - `end_mapping`: End SLAM mapping and save map
@@ -238,6 +372,12 @@ Supported system commands:
 2. Update MQTT command processing in `CustomRobot::processSportCmd()` or appropriate processor
 3. Add action documentation
 
+### Adding New Motion Switcher Actions
+
+1. Add action handler in `Controller` class (`include/controller.hpp`, `src/controller.cpp`) under MotionSwitcher section
+2. Update MQTT command processing in `CustomRobot::processMscCmd()`
+3. Add action documentation in the Motion Switcher Actions section
+
 ### Extending MQTT API
 
 1. Define new message types in the appropriate handler functions in `CustomRobot`