refactor(robot_control): 移除轨迹模式功能并添加连续移动控制

移除原有的复杂轨迹模式实现（包括正弦波、圆形、正方形等路径生成），
替换为更简单的连续移动控制接口，支持直接设置速度参数(vx,vy,vyaw)。
修改了相关线程控制和运动命令处理逻辑，简化了代码结构。

include/custom_robot.hpp

@@ -19,35 +19,6 @@
 
 namespace custom {
 
-/**
- * @brief 轨迹路径样式枚举
- */
-enum class TrajectoryPattern : int {
-    SINE_WAVE = 0,      // 正弦波（原有的波浪形）
-    CIRCLE = 1,         // 圆形
-    SQUARE = 2,         // 正方形
-    DIAMOND = 3,        // 菱形
-    FIGURE_EIGHT = 4    // 8字形
-};
-
-/**
- * @brief 轨迹参数结构
- */
-struct TrajectoryParams {
-    TrajectoryPattern pattern = TrajectoryPattern::SINE_WAVE;  // 路径样式
-    float scale = 1.0f;         // 缩放因子
-    float speed = 0.3f;         // 移动速度 (m/s)
-    float frequency = 1.0f;     // 频率参数（用于周期性路径）
-    float amplitude = 0.6f;     // 振幅参数
-    int points_count = 30;      // 路径点数量
-    float time_step = 0.06f;    // 时间步长
-    
-    // 构造函数
-    TrajectoryParams() = default;
-    TrajectoryParams(TrajectoryPattern p, float s = 1.0f, float sp = 0.3f) 
-        : pattern(p), scale(s), speed(sp) {}
-};
-
 class CustomRobot {
 public:
     explicit CustomRobot();
@@ -77,17 +48,12 @@ public:
     bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
     void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
     
-    bool startTraj(const TrajectoryParams& params = TrajectoryParams());
+    // Continuous movement control
-    void stopTraj();
+    bool startContinuousMove(float vx, float vy, float vyaw);
+    bool stopContinuousMove();
+    void continuousMoveLoop();
 
 private:
-    void trajControl();
-    std::vector<unitree::robot::go2::PathPoint> generateTrajectoryPath(const TrajectoryParams& params);
-    std::vector<unitree::robot::go2::PathPoint> generateSineWavePath(const TrajectoryParams& params);
-    std::vector<unitree::robot::go2::PathPoint> generateCirclePath(const TrajectoryParams& params);
-    std::vector<unitree::robot::go2::PathPoint> generateSquarePath(const TrajectoryParams& params);
-    std::vector<unitree::robot::go2::PathPoint> generateDiamondPath(const TrajectoryParams& params);
-    std::vector<unitree::robot::go2::PathPoint> generateFigureEightPath(const TrajectoryParams& params);
     std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
     // std::unique_ptr<LowController> low_controller_;
@@ -101,10 +67,12 @@ private:
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
     unitree::common::ThreadPtr traj_th_;
-    std::atomic<bool> traj_running_;
+    
-    float traj_dt_;
+    // Continuous movement control variables
-    float traj_count_;
+    std::atomic<bool> continuous_move_enabled_;
-    TrajectoryParams current_traj_params_;  // 当前轨迹参数
+    std::atomic<float> continuous_vx_;
+    std::atomic<float> continuous_vy_;
+    std::atomic<float> continuous_vyaw_;
 };
 
 } // namespace custom

src/custom_robot.cpp

@@ -21,9 +21,10 @@ CustomRobot::CustomRobot()
     , mqttClient_(nullptr)
     , running_(false)
     , initialized_(false)
-    , traj_running_(false)
+    , continuous_move_enabled_(false)
-    , traj_dt_(0.02f)
+    , continuous_vx_(0.0f)
-    , traj_count_(0.0f) {
+    , continuous_vy_(0.0f)
+    , continuous_vyaw_(0.0f) {
     config_.loadDefaults();
     
     try {
@@ -37,8 +38,6 @@ CustomRobot::CustomRobot()
 CustomRobot::~CustomRobot() {
     LOG_INFO("Shutting down CustomRobot");
 
-    stopTraj();
-
     if (mqttClient_) {
         mqttClient_->stopMessageProcessor();
         mqttClient_->disconnect();
@@ -150,81 +149,15 @@ bool CustomRobot::start() {
     // }
 
     running_ = true;
+    
+    // Start continuous movement thread
+    traj_th_ = unitree::common::CreateRecurrentThreadEx("continuous_move", UT_CPU_ID_NONE, 50, 
+        &CustomRobot::continuousMoveLoop, this);
+    
     LOG_INFO("CustomRobot started successfully");
     return true;
 }
 
-void CustomRobot::trajControl()
-{
-    if (!controller_) {
-        LOG_ERROR("Controller not available for dynamic trajectory.");
-        return;
-    }
-    
-    traj_count_ += traj_dt_;
-    std::vector<unitree::robot::go2::PathPoint> path = generateTrajectoryPath(current_traj_params_);
-
-    if (!controller_->TrajectoryFollow(path)) {
-        LOG_WARN("Failed to send dynamic trajectory command.");
-    }
-}
-
-bool CustomRobot::startTraj(const TrajectoryParams& params)
-{
-    if (traj_running_) {
-        LOG_WARN("Dynamic trajectory is already running.");
-        return true;
-    }
-
-    // 参数验证
-    if (params.time_step <= 0.0f || params.time_step > 1.0f) {
-        LOG_ERROR("Invalid time_step: " + std::to_string(params.time_step) + ". Must be between 0 and 1.0 seconds.");
-        return false;
-    }
-    
-    if (params.scale <= 0.0f) {
-        LOG_ERROR("Invalid scale: " + std::to_string(params.scale) + ". Must be positive.");
-        return false;
-    }
-    
-    if (params.speed <= 0.0f) {
-        LOG_ERROR("Invalid speed: " + std::to_string(params.speed) + ". Must be positive.");
-        return false;
-    }
-    
-    if (params.points_count <= 0) {
-        LOG_ERROR("Invalid points_count: " + std::to_string(params.points_count) + ". Must be positive.");
-        return false;
-    }
-
-    LOG_INFO("Starting dynamic trajectory control with pattern: " + std::to_string(static_cast<int>(params.pattern)) + 
-             ", time_step: " + std::to_string(params.time_step) + "s");
-    
-    current_traj_params_ = params;
-    traj_running_ = true;
-    traj_count_ = 0.0f;
-    
-    // 使用参数中的时间步长，转换为微秒
-    traj_dt_ = params.time_step;
-    int64_t interval_us = static_cast<int64_t>(traj_dt_ * 1000000);
-    
-    traj_th_ = unitree::common::CreateRecurrentThread(interval_us, std::bind(&CustomRobot::trajControl, this));
-    
-    return true;
-}
-
-void CustomRobot::stopTraj()
-{
-    if (traj_running_) {
-        LOG_INFO("Stopping dynamic trajectory control...");
-        traj_running_ = false;
-        if (traj_th_) {
-            traj_th_.reset();
-        }
-    }
-}
-
-
 
 bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
     if (!initialized_ || !rsc_) {
@@ -771,46 +704,22 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
             return controller_->WalkUpright(message["param"]["flag"]);
         } else if (cmd == "CrossStep") {
             return controller_->CrossStep(message["param"]["flag"]);
-        } else if (cmd == "start_traj") {
+        } else if (cmd == "StartContinuousMove") {
-            TrajectoryParams params;
+            if (!message.contains("param")) {
-            
+                LOG_ERROR("StartContinuousMove cmd missing 'param'");
-            // 从消息中解析参数（如果有的话）
+                return false;
-            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<TrajectoryPattern>(pattern_int);
             }
+            auto param = message["param"];
+            if (!param.contains("vx") || !param.contains("vy") || !param.contains("vyaw")) {
+                LOG_ERROR("StartContinuousMove missing vx, vy, or vyaw parameters");
+                return false;
             }
-                
+            float vx = param["vx"];
-                // 解析其他参数
+            float vy = param["vy"];
-                if (param.contains("scale")) {
+            float vyaw = param["vyaw"];
-                    params.scale = param["scale"];
+            return startContinuousMove(vx, vy, vyaw);
-                }
+        } else if (cmd == "StopContinuousMove") {
-                if (param.contains("speed")) {
+            return stopContinuousMove();
-                    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;
         } 
         else {
             LOG_ERROR("Unknown Sport command: " + cmd);
@@ -822,250 +731,44 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
     }
 }
 
-std::string CustomRobot::generateRandomClientId() const {
+// Continuous movement control implementation
-    std::string baseId = config_.mqtt_client_id;
+bool CustomRobot::startContinuousMove(float vx, float vy, float vyaw) {
-    
+    if (!controller_) {
-    std::string safeBaseId;
+        LOG_ERROR("Controller not initialized");
-    for (char c : baseId) {
+        return false;
-        if (std::isalnum(c) || c == '_' || c == '-') {
-            safeBaseId += c;
-        }
-    }
-    if (safeBaseId.empty()) {
-        safeBaseId = "unitree_go2_client";
     }
     
-    std::random_device rd;
+    continuous_vx_ = vx;
-    std::mt19937 gen(rd());
+    continuous_vy_ = vy;
-    std::uniform_int_distribution<> dis(1000, 9999);
+    continuous_vyaw_ = vyaw;
+    continuous_move_enabled_ = true;
     
-    std::stringstream ss;
+    LOG_INFO("Started continuous move with vx=" + std::to_string(vx) + 
-    ss << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen);
+             ", vy=" + std::to_string(vy) + ", vyaw=" + std::to_string(vyaw));
-    
+    return true;
-    return ss.str();
 }
 
-// ==================== 轨迹路径生成函数 ====================
+bool CustomRobot::stopContinuousMove() {
+    continuous_move_enabled_ = false;
     
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateTrajectoryPath(const TrajectoryParams& params)
+    if (controller_) {
-{
+        controller_->StopMove();
-    switch (params.pattern) {
-        case TrajectoryPattern::CIRCLE:
-            return generateCirclePath(params);
-        case TrajectoryPattern::SQUARE:
-            return generateSquarePath(params);
-        case TrajectoryPattern::DIAMOND:
-            return generateDiamondPath(params);
-        case TrajectoryPattern::FIGURE_EIGHT:
-            return generateFigureEightPath(params);
-        case TrajectoryPattern::SINE_WAVE:
-        default:
-            return generateSineWavePath(params);
-    }
     }
     
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateSineWavePath(const TrajectoryParams& params)
+    LOG_INFO("Stopped continuous move");
-{
+    return true;
-    std::vector<unitree::robot::go2::PathPoint> path;
-    float delta = params.time_step;
-    
-    for (int i = 0; i < params.points_count; i++) {
-        unitree::robot::go2::PathPoint p;
-        float var = (traj_count_ + i * delta);
-        p.timeFromStart = i * delta;
-        p.x = params.speed * var * params.scale;
-        p.y = params.amplitude * params.scale * sin(M_PI * params.frequency * params.speed * var);
-        p.yaw = 2 * params.amplitude * params.scale * params.speed * M_PI * params.frequency * cos(M_PI * params.frequency * params.speed * var);
-        p.vx = params.speed;
-        p.vy = M_PI * params.frequency * params.speed * (params.amplitude * params.scale * cos(M_PI * params.frequency * params.speed * var));
-        p.vyaw = -M_PI * params.frequency * params.speed * 2 * params.amplitude * params.scale * params.speed * M_PI * params.frequency * sin(M_PI * params.frequency * params.speed * var);
-        path.push_back(p);
 }
 
-    return path;
+void CustomRobot::continuousMoveLoop() {
+    if (!continuous_move_enabled_ || !controller_) {
+        return;
     }
     
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateCirclePath(const TrajectoryParams& params)
+    float vx = continuous_vx_.load();
-{
+    float vy = continuous_vy_.load();
-    std::vector<unitree::robot::go2::PathPoint> path;
+    float vyaw = continuous_vyaw_.load();
-    float delta = params.time_step;
-    float radius = params.amplitude * params.scale;
-    float angular_velocity = params.frequency * params.speed / radius;
     
-    for (int i = 0; i < params.points_count; i++) {
+    controller_->Move(vx, vy, vyaw);
-        unitree::robot::go2::PathPoint p;
-        float t = traj_count_ + i * delta;
-        float angle = angular_velocity * t;
-        
-        p.timeFromStart = i * delta;
-        p.x = radius * cos(angle);
-        p.y = radius * sin(angle);
-        p.yaw = angle + M_PI / 2;  // 切线方向
-        p.vx = -radius * angular_velocity * sin(angle);
-        p.vy = radius * angular_velocity * cos(angle);
-        p.vyaw = angular_velocity;
-        path.push_back(p);
-    }
-    
-    return path;
-}
-
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateSquarePath(const TrajectoryParams& params)
-{
-    std::vector<unitree::robot::go2::PathPoint> path;
-    float delta = params.time_step;
-    float side_length = params.amplitude * params.scale * 2;  // 正方形边长
-    float perimeter = side_length * 4;
-    float speed = params.speed;
-    
-    for (int i = 0; i < params.points_count; i++) {
-        unitree::robot::go2::PathPoint p;
-        float t = traj_count_ + i * delta;
-        float distance = fmod(speed * t * params.frequency, perimeter);  // 沿周长的距离
-        
-        p.timeFromStart = i * delta;
-        
-        // 确定在正方形的哪一边
-        if (distance < side_length) {
-            // 底边：从左到右
-            p.x = -side_length/2 + distance;
-            p.y = -side_length/2;
-            p.yaw = 0;
-            p.vx = speed;
-            p.vy = 0;
-        } else if (distance < 2 * side_length) {
-            // 右边：从下到上
-            p.x = side_length/2;
-            p.y = -side_length/2 + (distance - side_length);
-            p.yaw = M_PI/2;
-            p.vx = 0;
-            p.vy = speed;
-        } else if (distance < 3 * side_length) {
-            // 顶边：从右到左
-            p.x = side_length/2 - (distance - 2 * side_length);
-            p.y = side_length/2;
-            p.yaw = M_PI;
-            p.vx = -speed;
-            p.vy = 0;
-        } else {
-            // 左边：从上到下
-            p.x = -side_length/2;
-            p.y = side_length/2 - (distance - 3 * side_length);
-            p.yaw = -M_PI/2;
-            p.vx = 0;
-            p.vy = -speed;
-        }
-        
-        p.vyaw = 0;  // 正方形路径角速度为0（除了转角处）
-        path.push_back(p);
-    }
-    
-    return path;
-}
-
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateDiamondPath(const TrajectoryParams& params)
-{
-    std::vector<unitree::robot::go2::PathPoint> path;
-    float delta = params.time_step;
-    float size = params.amplitude * params.scale;  // 菱形大小
-    float perimeter = size * 4 * sqrt(2);  // 菱形周长
-    float speed = params.speed;
-    
-    for (int i = 0; i < params.points_count; i++) {
-        unitree::robot::go2::PathPoint p;
-        float t = traj_count_ + i * delta;
-        float distance = fmod(speed * t * params.frequency, perimeter);
-        
-        p.timeFromStart = i * delta;
-        
-        float side_length = size * sqrt(2);
-        
-        // 确定在菱形的哪一边
-        if (distance < side_length) {
-            // 右下边
-            float progress = distance / side_length;
-            p.x = size * progress;
-            p.y = -size * (1 - progress);
-            p.yaw = M_PI/4;
-            p.vx = speed * cos(M_PI/4);
-            p.vy = speed * sin(M_PI/4);
-        } else if (distance < 2 * side_length) {
-            // 右上边
-            float progress = (distance - side_length) / side_length;
-            p.x = size * (1 - progress);
-            p.y = size * progress;
-            p.yaw = 3*M_PI/4;
-            p.vx = -speed * cos(M_PI/4);
-            p.vy = speed * sin(M_PI/4);
-        } else if (distance < 3 * side_length) {
-            // 左上边
-            float progress = (distance - 2 * side_length) / side_length;
-            p.x = -size * progress;
-            p.y = size * (1 - progress);
-            p.yaw = -3*M_PI/4;
-            p.vx = -speed * cos(M_PI/4);
-            p.vy = -speed * sin(M_PI/4);
-        } else {
-            // 左下边
-            float progress = (distance - 3 * side_length) / side_length;
-            p.x = -size * (1 - progress);
-            p.y = -size * progress;
-            p.yaw = -M_PI/4;
-            p.vx = speed * cos(M_PI/4);
-            p.vy = -speed * sin(M_PI/4);
-        }
-        
-        p.vyaw = 0;
-        path.push_back(p);
-    }
-    
-    return path;
-}
-
-std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateFigureEightPath(const TrajectoryParams& params)
-{
-    std::vector<unitree::robot::go2::PathPoint> path;
-    float delta = params.time_step;
-    float radius = params.amplitude * params.scale;
-    float angular_velocity = params.frequency * params.speed / radius;
-    
-    for (int i = 0; i < params.points_count; i++) {
-        unitree::robot::go2::PathPoint p;
-        float t = traj_count_ + i * delta;
-        float angle = angular_velocity * t;
-        
-        p.timeFromStart = i * delta;
-        
-        // 8字形参数方程
-        float sin_t = sin(angle);
-        float cos_t = cos(angle);
-        float sin_2t = sin(2 * angle);
-        
-        p.x = radius * sin_t;
-        p.y = radius * sin_2t / 2;
-        
-        // 计算切线方向
-        float dx_dt = radius * cos_t * angular_velocity;
-        float dy_dt = radius * cos(2 * angle) * angular_velocity;
-        p.yaw = atan2(dy_dt, dx_dt);
-        
-        p.vx = dx_dt;
-        p.vy = dy_dt;
-        
-        // 计算角速度
-        float speed_magnitude = sqrt(dx_dt * dx_dt + dy_dt * dy_dt);
-        if (speed_magnitude > 0.001f) {
-            float d2x_dt2 = -radius * sin_t * angular_velocity * angular_velocity;
-            float d2y_dt2 = -2 * radius * sin_2t * angular_velocity * angular_velocity;
-            p.vyaw = (dx_dt * d2y_dt2 - dy_dt * d2x_dt2) / (speed_magnitude * speed_magnitude);
-        } else {
-            p.vyaw = 0;
-        }
-        
-        path.push_back(p);
-    }
-    
-    return path;
 }
 
 } // namespace custom