refactor(robot_control): 移除轨迹模式功能并添加连续移动控制
移除原有的复杂轨迹模式实现(包括正弦波、圆形、正方形等路径生成), 替换为更简单的连续移动控制接口,支持直接设置速度参数(vx,vy,vyaw)。 修改了相关线程控制和运动命令处理逻辑,简化了代码结构。
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user