tanjianbin/lzwc-terminal-unitreeGo2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
移除原有的复杂轨迹模式实现（包括正弦波、圆形、正方形等路径生成），
替换为更简单的连续移动控制接口，支持直接设置速度参数(vx,vy,vyaw)。
修改了相关线程控制和运动命令处理逻辑，简化了代码结构。
This commit is contained in:
Sucan126
2025-10-09 19:17:26 +08:00
parent 06bf2fa208
commit 2e60da5b5b
2 changed files with 64 additions and 393 deletions
Download Patch File Download Diff File Expand all files Collapse all files

403
src/custom_robot.cpp
View File

@@ -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<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,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<TrajectoryPattern>(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<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);
bool CustomRobot::stopContinuousMove() {
continuous_move_enabled_ = false;
if (controller_) {
controller_->StopMove();
}
LOG_INFO("Stopped continuous move");
return true;
}
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);
void CustomRobot::continuousMoveLoop() {
if (!continuous_move_enabled_ || !controller_) {
return;
}
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;
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<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;
controller_->Move(vx, vy, vyaw);
}
} // namespace custom