feat(轨迹控制): 添加多种轨迹模式支持并重构轨迹生成逻辑

新增轨迹模式枚举和参数结构体，支持正弦波、圆形、正方形、菱形和8字形五种轨迹模式
重构轨迹生成逻辑为独立的生成函数，增加参数验证和配置灵活性

include/custom_robot.hpp

@@ -6,8 +6,8 @@
 #include "mqtt.hpp"
 #include "navigation.hpp"
 #include "recharge.hpp"
-#include "monitor.hpp"
 // #include "low_controller.hpp"
+#include "monitor.hpp"
 
 #include <memory>
 #include <atomic>
@@ -19,6 +19,35 @@
 
 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();
@@ -48,11 +77,17 @@ 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();
+    bool startTraj(const TrajectoryParams& params = TrajectoryParams());
     void stopTraj();
 
 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_;
@@ -69,6 +104,7 @@ private:
     std::atomic<bool> traj_running_;
     float traj_dt_;
     float traj_count_;
+    TrajectoryParams current_traj_params_;  // 当前轨迹参数
 };
 
 } // namespace custom

src/custom_robot.cpp

@@ -160,40 +160,55 @@ void CustomRobot::trajControl()
         LOG_ERROR("Controller not available for dynamic trajectory.");
         return;
     }
-    float vx = 0.3f;
-    float delta = 0.06f;
+    
     traj_count_ += traj_dt_;
-    std::vector<unitree::robot::go2::PathPoint> path;
-    for (int i=0; i<30; i++) {
-        unitree::robot::go2::PathPoint p;
-        float var = (traj_count_ + i * delta);
-        p.timeFromStart = i * delta;
-        p.x = vx * var;
-        p.y = 0.6 * sin(M_PI * vx * var);
-        p.yaw = 2 * 0.6 * vx * M_PI * cos(M_PI * vx * var);
-        p.vx = vx;
-        p.vy = M_PI * vx * (0.6 * cos(M_PI * vx * var));
-        p.vyaw = - M_PI * vx * 2 * 0.6 * vx * M_PI * sin(M_PI * vx * var);
-        path.push_back(p);
-    }
+    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()
+bool CustomRobot::startTraj(const TrajectoryParams& params)
 {
     if (traj_running_) {
         LOG_WARN("Dynamic trajectory is already running.");
         return true;
     }
 
-    LOG_INFO("Starting dynamic trajectory control...");
+    // 参数验证
+    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_th_ = unitree::common::CreateRecurrentThread(static_cast<int64_t>(traj_dt_ * 1000000), std::bind(&CustomRobot::trajControl, this));
+    // 使用参数中的时间步长，转换为微秒
+    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;
 }
@@ -209,6 +224,8 @@ void CustomRobot::stopTraj()
     }
 }
 
+
+
 bool CustomRobot::GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList) {
     if (!initialized_ || !rsc_) {
         LOG_ERROR("Robot not initialized or stateClient is null");
@@ -793,4 +810,227 @@ std::string CustomRobot::generateRandomClientId() const {
     return ss.str();
 }
 
+// ==================== 轨迹路径生成函数 ====================
+
+std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateTrajectoryPath(const TrajectoryParams& params)
+{
+    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)
+{
+    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;
+}
+
+std::vector<unitree::robot::go2::PathPoint> CustomRobot::generateCirclePath(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;
+        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