111 lines
4.1 KiB
C++
111 lines
4.1 KiB
C++
#pragma once
|
|
|
|
#include "controller.hpp"
|
|
#include "config.hpp"
|
|
#include "logger.hpp"
|
|
#include "mqtt.hpp"
|
|
#include "navigation.hpp"
|
|
#include "recharge.hpp"
|
|
// #include "low_controller.hpp"
|
|
#include "monitor.hpp"
|
|
|
|
#include <memory>
|
|
#include <atomic>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <nlohmann/json.hpp>
|
|
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
|
|
#include <unitree/common/thread/recurrent_thread.hpp>
|
|
|
|
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();
|
|
~CustomRobot();
|
|
|
|
bool initialize();
|
|
bool start();
|
|
|
|
Controller* getController() const { return controller_.get(); }
|
|
Navigation* getNavigation() const { return navigation_.get(); }
|
|
|
|
bool GetServiceList(std::vector<unitree::robot::go2::ServiceState>& serviceList);
|
|
bool SwitchService(const std::string& serviceName, bool enable);
|
|
bool SetReportFreq(int32_t interval, int32_t duration);
|
|
|
|
bool initializeMqtt();
|
|
void onMqttMessage(const std::string& topic, const std::string& payload);
|
|
void onMqttConnection(bool connected);
|
|
|
|
void processCmd(const nlohmann::json& message);
|
|
bool processOacCmd(const std::string& cmd, const nlohmann::json& message);
|
|
bool processSportCmd(const std::string& cmd, const nlohmann::json& message);
|
|
bool processRscCmd(const std::string& cmd, const nlohmann::json& message);
|
|
bool processNavCmd(const std::string& cmd, const nlohmann::json& message);
|
|
bool processMscCmd(const std::string& cmd, const nlohmann::json& message);
|
|
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();
|
|
|
|
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_;
|
|
std::unique_ptr<Navigation> navigation_;
|
|
std::unique_ptr<unitree::robot::go2::RobotStateClient> rsc_;
|
|
std::unique_ptr<MqttClient> mqttClient_;
|
|
std::unique_ptr<Recharge> recharge_;
|
|
std::unique_ptr<Monitor> monitor_;
|
|
|
|
CustomConfig config_;
|
|
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_; // 当前轨迹参数
|
|
};
|
|
|
|
} // namespace custom
|