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

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

View File

@@ -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<unitree::robot::go2::ServiceState>& 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<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_;
float traj_count_;
TrajectoryParams current_traj_params_; // 当前轨迹参数
// Continuous movement control variables
std::atomic<bool> continuous_move_enabled_;
std::atomic<float> continuous_vx_;
std::atomic<float> continuous_vy_;
std::atomic<float> continuous_vyaw_;
};
} // namespace custom