From 993ed111a643cb528c7837c1ecad956d1359760e Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Thu, 9 Oct 2025 17:49:08 +0800 Subject: [PATCH] =?UTF-8?q?feat(=E8=BD=A8=E8=BF=B9=E6=8E=A7=E5=88=B6):=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=A4=9A=E7=A7=8D=E8=BD=A8=E8=BF=B9=E6=A8=A1?= =?UTF-8?q?=E5=BC=8F=E6=94=AF=E6=8C=81=E5=B9=B6=E9=87=8D=E6=9E=84=E8=BD=A8?= =?UTF-8?q?=E8=BF=B9=E7=94=9F=E6=88=90=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增轨迹模式枚举和参数结构体，支持正弦波、圆形、正方形、菱形和8字形五种轨迹模式 重构轨迹生成逻辑为独立的生成函数，增加参数验证和配置灵活性 --- include/custom_robot.hpp | 40 +++++- src/custom_robot.cpp | 276 ++++++++++++++++++++++++++++++++++++--- 2 files changed, 296 insertions(+), 20 deletions(-) diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index ed31a91..65e67e6 100644 --- a/include/custom_robot.hpp +++ b/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 #include @@ -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& serviceList, int filterStatus = -1); - bool startTraj(); + bool startTraj(const TrajectoryParams& params = TrajectoryParams()); void stopTraj(); private: void trajControl(); + std::vector generateTrajectoryPath(const TrajectoryParams& params); + std::vector generateSineWavePath(const TrajectoryParams& params); + std::vector generateCirclePath(const TrajectoryParams& params); + std::vector generateSquarePath(const TrajectoryParams& params); + std::vector generateDiamondPath(const TrajectoryParams& params); + std::vector generateFigureEightPath(const TrajectoryParams& params); std::string generateRandomClientId() const; std::unique_ptr controller_; // std::unique_ptr low_controller_; @@ -69,6 +104,7 @@ private: std::atomic traj_running_; float traj_dt_; float traj_count_; + TrajectoryParams current_traj_params_; // 当前轨迹参数 }; } // namespace custom diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 50e6910..9c92ba4 100644 --- a/src/custom_robot.cpp +++ b/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 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 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(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(traj_dt_ * 1000000), std::bind(&CustomRobot::trajControl, this)); + // 使用参数中的时间步长，转换为微秒 + traj_dt_ = params.time_step; + int64_t interval_us = static_cast(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& 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 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 CustomRobot::generateSineWavePath(const TrajectoryParams& params) +{ + std::vector 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 CustomRobot::generateCirclePath(const TrajectoryParams& params) +{ + std::vector 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 CustomRobot::generateSquarePath(const TrajectoryParams& params) +{ + std::vector 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 CustomRobot::generateDiamondPath(const TrajectoryParams& params) +{ + std::vector 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 CustomRobot::generateFigureEightPath(const TrajectoryParams& params) +{ + std::vector 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