diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 9c92ba4..33ea928 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -772,7 +772,42 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& } else if (cmd == "CrossStep") { return controller_->CrossStep(message["param"]["flag"]); } else if (cmd == "start_traj") { - return startTraj(); + TrajectoryParams params; + + // 从消息中解析参数(如果有的话) + if (message.contains("param")) { + const auto& param = message["param"]; + + // 解析轨迹模式 + if (param.contains("pattern")) { + int pattern_int = param["pattern"]; + if (pattern_int >= 0 && pattern_int <= 4) { + params.pattern = static_cast(pattern_int); + } + } + + // 解析其他参数 + if (param.contains("scale")) { + params.scale = param["scale"]; + } + if (param.contains("speed")) { + params.speed = param["speed"]; + } + if (param.contains("frequency")) { + params.frequency = param["frequency"]; + } + if (param.contains("amplitude")) { + params.amplitude = param["amplitude"]; + } + if (param.contains("points_count")) { + params.points_count = param["points_count"]; + } + if (param.contains("time_step")) { + params.time_step = param["time_step"]; + } + } + + return startTraj(params); } else if (cmd == "stop_traj") { stopTraj(); return true;