From 2e60da5b5b099b068406ba91f3f8d34d206e6d7e Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Thu, 9 Oct 2025 19:17:26 +0800 Subject: [PATCH] =?UTF-8?q?refactor(robot=5Fcontrol):=20=E7=A7=BB=E9=99=A4?= =?UTF-8?q?=E8=BD=A8=E8=BF=B9=E6=A8=A1=E5=BC=8F=E5=8A=9F=E8=83=BD=E5=B9=B6?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=BF=9E=E7=BB=AD=E7=A7=BB=E5=8A=A8=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 移除原有的复杂轨迹模式实现（包括正弦波、圆形、正方形等路径生成）， 替换为更简单的连续移动控制接口，支持直接设置速度参数(vx,vy,vyaw)。 修改了相关线程控制和运动命令处理逻辑，简化了代码结构。 --- include/custom_robot.hpp | 54 ++---- src/custom_robot.cpp | 403 +++++---------------------------------- 2 files changed, 64 insertions(+), 393 deletions(-) diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index 65e67e6..b84ad17 100644 --- a/include/custom_robot.hpp +++ b/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(); @@ -76,18 +47,13 @@ public: bool processLowCmd(const std::string& cmd, const nlohmann::json& message); bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message); void printServiceList(const std::vector& serviceList, int filterStatus = -1); - - bool startTraj(const TrajectoryParams& params = TrajectoryParams()); - void stopTraj(); + + // Continuous movement control + bool startContinuousMove(float vx, float vy, float vyaw); + bool stopContinuousMove(); + void continuousMoveLoop(); 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_; @@ -101,10 +67,12 @@ private: std::atomic running_; std::atomic initialized_; unitree::common::ThreadPtr traj_th_; - std::atomic traj_running_; - float traj_dt_; - float traj_count_; - TrajectoryParams current_traj_params_; // 当前轨迹参数 + + // Continuous movement control variables + std::atomic continuous_move_enabled_; + std::atomic continuous_vx_; + std::atomic continuous_vy_; + std::atomic continuous_vyaw_; }; } // namespace custom diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 33ea928..c8399be 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -20,10 +20,11 @@ CustomRobot::CustomRobot() , rsc_(nullptr) , mqttClient_(nullptr) , running_(false) - , initialized_(false) - , traj_running_(false) - , traj_dt_(0.02f) - , traj_count_(0.0f) { + , initialized_(false) + , continuous_move_enabled_(false) + , continuous_vx_(0.0f) + , continuous_vy_(0.0f) + , continuous_vyaw_(0.0f) { config_.loadDefaults(); try { @@ -36,8 +37,6 @@ CustomRobot::CustomRobot() CustomRobot::~CustomRobot() { LOG_INFO("Shutting down CustomRobot"); - - stopTraj(); if (mqttClient_) { mqttClient_->stopMessageProcessor(); @@ -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 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(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(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& serviceList) { if (!initialized_ || !rsc_) { @@ -771,47 +704,23 @@ 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") { - 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"]; - } + } else if (cmd == "StartContinuousMove") { + if (!message.contains("param")) { + LOG_ERROR("StartContinuousMove cmd missing 'param'"); + return false; } - - return startTraj(params); - } else if (cmd == "stop_traj") { - stopTraj(); - return true; - } + 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"]; + float vyaw = param["vyaw"]; + return startContinuousMove(vx, vy, vyaw); + } else if (cmd == "StopContinuousMove") { + return stopContinuousMove(); + } else { LOG_ERROR("Unknown Sport command: " + cmd); return false; @@ -822,250 +731,44 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& } } -std::string CustomRobot::generateRandomClientId() const { - std::string baseId = config_.mqtt_client_id; - - std::string safeBaseId; - for (char c : baseId) { - if (std::isalnum(c) || c == '_' || c == '-') { - safeBaseId += c; - } - } - if (safeBaseId.empty()) { - safeBaseId = "unitree_go2_client"; +// Continuous movement control implementation +bool CustomRobot::startContinuousMove(float vx, float vy, float vyaw) { + if (!controller_) { + LOG_ERROR("Controller not initialized"); + return false; } - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution<> dis(1000, 9999); + continuous_vx_ = vx; + continuous_vy_ = vy; + continuous_vyaw_ = vyaw; + continuous_move_enabled_ = true; - std::stringstream ss; - ss << safeBaseId << "_" << std::time(nullptr) << "_" << dis(gen); - - return ss.str(); + LOG_INFO("Started continuous move with vx=" + std::to_string(vx) + + ", vy=" + std::to_string(vy) + ", vyaw=" + std::to_string(vyaw)); + return true; } -// ==================== 轨迹路径生成函数 ==================== - -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); +bool CustomRobot::stopContinuousMove() { + continuous_move_enabled_ = false; + + if (controller_) { + controller_->StopMove(); } + + LOG_INFO("Stopped continuous move"); + return true; } -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); +void CustomRobot::continuousMoveLoop() { + if (!continuous_move_enabled_ || !controller_) { + return; } - 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; + float vx = continuous_vx_.load(); + float vy = continuous_vy_.load(); + float vyaw = continuous_vyaw_.load(); - 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; + controller_->Move(vx, vy, vyaw); } } // namespace custom